#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif

#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <ctype.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sstream>
#include "rs274ngc.hh"
#include "rs274ngc_return.hh"
#include "interp_internal.hh"
#include "rs274ngc_interp.hh"
#include "interp_queue.hh"
#include "rtapi/rtapi_math.h"
#include <cmath>
#include <time.h>

#include "rtapi/rtapi.h"
#include <vector>
#include <algorithm>
#include "motion/usrmotintf.h"
#include "nml_intf/zuc.hh"  // ZUC NML
#include "nml_intf/zuc_nml.hh"
#include "nml_intf/interpl.hh"
#include <unistd.h>
#include "_timer.h" /* etime(), esleep() */
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <regex>
#include <iostream>
#include <set>
#include <boost/tokenizer.hpp>
#include "log/zuclog.h"
#include <string>
#include "nml_intf/interp_return.hh"
#include <sstream>
#include <fstream>
#include <regex>
#include "nml_intf/interpl.hh"
#include <mutex>

void stringSplit(std::string& inString, std::vector<std::string>& outVector, std::string inSplit)
{
    std::string::size_type pos1, pos2;
    size_t len = inString.length();
    pos2 = inString.find(inSplit);
    pos1 = 0;

    while (std::string::npos != pos2)
    {
        outVector.emplace_back(inString.substr(pos1, pos2 - pos1));
        pos1 = pos2 + inSplit.size();
        pos2 = inString.find(inSplit, pos1);
    }

    if (pos1 != len)
        outVector.emplace_back(inString.substr(pos1));
}

static int isToken(const char* line, int* counter, const char* token)
{
    unsigned int endIdx = (*counter + strlen(token));
    if (strncmp(line + *counter, token, strlen(token)) == 0 && (endIdx <= strlen(line)) &&
        (endIdx == strlen(line) || line[endIdx] == '(' || line[endIdx] == '[' || line[endIdx] == ' ' || line[endIdx] == ':'))
    {
        *counter += strlen(token);
        return 1;
    }
    return 0;
}

static int skipBlank(const char* line, int* counter)
{
    if (line == nullptr || counter == nullptr)
    {
        return -1;
    }
    while ((*counter < (int)strlen(line) && (line[*counter] == ' ')) || (line[*counter] == ' ') || (line[*counter] == '\t')) *counter = *counter + 1;
    return 0;
}

static std::string doubleToString(double data)
{
    std::string res = std::to_string(data);
    res = res.substr(0, res.find_last_not_of('0') + 1);
    if (res.back() == '.')
    {
        res.erase(res.end() - 1);
    }
    return res;
}

static inline std::string& ltrim(std::string& s)
{
    s.erase(s.begin(), std::find_if(s.begin(), s.end(), [](int ch) { return !std::isspace(ch); }));
    return s;
}

static inline std::string& rtrim(std::string& s)
{
    s.erase(std::find_if(s.rbegin(), s.rend(), [](int ch) { return !std::isspace(ch); }).base(), s.end());
    return s;
}

static inline std::string& trim(std::string& s) { return ltrim(rtrim(s)); }

// synchronize the parsing and execution
/*
#define SYNCH_PARSE_EXEC                                                                                                                              \
    do                                                                                                                                                \
    {                                                                                                                                                 \
        printf(">>>>>>>>>>>>>interpState = %d   taskState=%d     interp_list.len()=%d\n", interpSynchState, GET_TASK_EXECSTATE(), interp_list.len()); \
        if (interpSynchState == 0)                                                                                                                    \
        {                                                                                                                                             \
            waitFlag = 1;                                                                                                                             \
            interpSynchState = 1;                                                                                                                     \
            enqueue_DWELL(0, 0.001);                                                                                                                  \
            --_setup.sequence_number;                                                                                                                 \
            *(counter) = strlen(line);                                                                                                                \
            return INTERP_SYNCH;                                                                                                                      \
        }                                                                                                                                             \
        else if (interpSynchState == 1)                                                                                                               \
        {                                                                                                                                             \
            if (interp_list.len() > 0 || GET_TASK_EXECSTATE() != 2)                                                                                   \
            {                                                                                                                                         \
                waitFlag = 1;                                                                                                                         \
                interpSynchState = 2;                                                                                                                 \
                enqueue_DWELL(0, 0.001);                                                                                                              \
                --_setup.sequence_number;                                                                                                             \
                *(counter) = strlen(line);                                                                                                            \
                return INTERP_SYNCH;                                                                                                                  \
            }                                                                                                                                         \
            else                                                                                                                                      \
            {                                                                                                                                         \
                interpSynchState = 0;                                                                                                                 \
            }                                                                                                                                         \
        }                                                                                                                                             \
        else if (interpSynchState == 2)                                                                                                               \
        {                                                                                                                                             \
            interpSynchState = 0;                                                                                                                     \
        }                                                                                                                                             \
    } while (0)
*/

#define SYNCH_PARSE_EXEC                                                                                                                                                      \
    do {                                                                                                                                                                      \
        if (interpSynchState == 0)                                                                                                                                            \
        {                                                                                                                                                                     \
            _waitFlag = 1;                                                                                                                                                    \
            interpSynchState = 1;                                                                                                                                             \
            enqueue_DWELL(0, 0.001, m_threadId);                                                                                                                              \
            --_setup.sequence_number;                                                                                                                                         \
            *(counter) = strlen(line);                                                                                                                                        \
            return INTERP_SYNCH;                                                                                                                                              \
        }                                                                                                                                                                     \
        else if (interpSynchState == 1)                                                                                                                                       \
        {                                                                                                                                                                     \
            if (m_threadId == 0)                                                                                                                                              \
            {                                                                                                                                                                 \
                if ((_setup.sequence_number > 1 && interp_list.len() > 0) || GET_TASK_EXECSTATE() != 2)                                                                       \
                /*避免jks的第一条指令就SYNCH_PARSE_EXEC出异常卡死，因为TOPPRA 会在主jks第一行执行TOPPRA_FIRST_COMMAND()，会插入一条nml指令*/ \
                {                                                                                                                                                             \
                    _waitFlag = 1;                                                                                                                                            \
                    /* interpSynchState = 2;  */                                                                                                                              \
                    enqueue_DWELL(0, 0.001, m_threadId);                                                                                                                      \
                    --_setup.sequence_number;                                                                                                                                 \
                    *(counter) = strlen(line);                                                                                                                                \
                    return INTERP_SYNCH;                                                                                                                                      \
                }                                                                                                                                                             \
                else                                                                                                                                                          \
                {                                                                                                                                                             \
                    interpSynchState = 0;                                                                                                                                     \
                }                                                                                                                                                             \
            }                                                                                                                                                                 \
            else if (m_threadId > 0)                                                                                                                                          \
            {                                                                                                                                                                 \
                if (threadInterpList[m_threadId - 1].nmlcmd_list->len() > 0)                                                                                                  \
                {                                                                                                                                                             \
                    _waitFlag = 1;                                                                                                                                            \
                    /* interpSynchState = 2;  */                                                                                                                              \
                    enqueue_DWELL(0, 0.001, m_threadId);                                                                                                                      \
                    --_setup.sequence_number;                                                                                                                                 \
                    *(counter) = strlen(line);                                                                                                                                \
                    return INTERP_SYNCH;                                                                                                                                      \
                }                                                                                                                                                             \
                else                                                                                                                                                          \
                {                                                                                                                                                             \
                    interpSynchState = 0;                                                                                                                                     \
                }                                                                                                                                                             \
            }                                                                                                                                                                 \
        }                                                                                                                                                                     \
    } while (0)

// wait on current line for some condition
#define WAIT_PARSE_EXEC            \
    do {                           \
        --_setup.sequence_number;  \
        *(counter) = strlen(line); \
        return INTERP_SYNCH;       \
    } while (0)

#define CHK_INTERP_READ(ret, line_ptr, counter_ptr) \
    do {                                            \
        if (ret == INTERP_SYNCH)                    \
        {                                           \
            *counter_ptr = strlen(line_ptr);        \
            return ret;                             \
        }                                           \
        else                                        \
        {                                           \
            CHP(ret);                               \
        }                                           \
    } while (0)

int Interp::set_prog_start_line(int start_line)
{
    progStartLine = start_line;
    return 0;
}

int Interp::update_start_line(setup_pointer settings)
{
    // rtapi_print("subfile size: %d, seqNum: %d, progStartLine: %d\n", settings->sub_file_pointer.size(),
    // 			settings->sequence_number, progStartLine);
    if (settings->sub_file_pointer.size() == 0)
    {
        if (settings->sequence_number < progStartLine)
        {
            beforeProgStartLine = true;
        }
        else
        {
            beforeProgStartLine = false;
            if (progStartLine > 0)
            {
                progStartLine = 0;
            }
        }
    }
    return 0;
}

int Interp::set_parsing_flag(bool flag)
{
    parsing_flag = flag;
    return 0;
}

int Interp::set_endforcecond_flag(int flag_value)
{
    endForceCondFlag = flag_value;
    return 0;
}
/****************************************************************************/

/*! read_a

 Returned Value: int
 If read_real_value returns an error code, this returns that code.
 If any of the following errors occur, this returns the error code shown.
 Otherwise, it returns INTERP_OK.
 1. The first character read is not a:
 NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED
 2. An a_coordinate has already been inserted in the block:
 NCE_MULTIPLE_A_WORDS_ON_ONE_LINE.
 3. A values are not allowed: NCE_CANNOT_USE_A_WORD.

 Side effects:
 counter is reset.
 The a_flag in the block is turned on.
 An a_number is inserted in the block.

 Called by: read_one_item

 When this function is called, counter is pointing at an item on the
 line that starts with the character 'a', indicating an a_coordinate
 setting. The function reads characters which tell how to set the
 coordinate, up to the start of the next item or the end of the line.
 The counter is then set to point to the character following.

 The value may be a real number or something that evaluates to a
 real number, so read_real_value is used to read it. Parameters
 may be involved.

 If the AA compiler flag is defined, the a_flag in the block is turned
 on and the a_number in the block is set to the value read. If the
 AA flag is not defined, (i) if the AXIS_ERROR flag is defined, that means
 A values are not allowed, and an error value is returned, (ii) if the
 AXIS_ERROR flag is not defined, nothing is done.

 */

int Interp::read_a(char* line,           //!< string: line of RS274/NGC code being processed
                   int* counter,         //!< pointer to a counter for position on the line
                   block_pointer block,  //!< pointer to a block being filled from the line
                   double* parameters)   //!< array of system parameters
{
    jaka_value value;

    CHKS((line[*counter] != 'a'), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = (*counter + 1);
    CHKS((block->a_flag), INTERP_ERR_SYNTAX_FAILED, NCE_MULTIPLE_A_WORDS_ON_ONE_LINE);
    CHP(read_real_value(line, counter, &value, parameters));
    block->a_flag = true;
    block->a_number = value.scalarValue;
    return INTERP_OK;
}

/****************************************************************************/

/*! read_atan

 Returned Value: int
 If read_real_expression returns an error code, this returns that code.
 If any of the following errors occur, this returns the error code shown.
 Otherwise, it returns INTERP_OK.
 1. The first character to read is not a slash:
 NCE_SLASH_MISSING_AFTER_FIRST_ATAN_ARGUMENT
 2. The second character to read is not a left bracket:
 NCE_LEFT_BRACKET_MISSING_AFTER_SLASH_WITH_ATAN

 Side effects:
 The computed value is put into what double_ptr points at.
 The counter is reset to point to the first character after the
 characters which make up the value.

 Called by:
 read_unary

 When this function is called, the characters "atan" and the first
 argument have already been read, and the value of the first argument
 is stored in double_ptr.  This function attempts to read a slash and
 the second argument to the atan function, starting at the index given
 by the counter and then to compute the value of the atan operation
 applied to the two arguments.  The computed value is inserted into
 what double_ptr points at.

 The computed value is in the range from -180 degrees to +180 degrees.
 The range is not specified in the RS274/NGC manual [NCMS, page 51],
 although using degrees (not radians) is specified.

 */
int Interp::read_convey_linear_enable(char* line,           //!< string: line of RS274/NGC code being processed
                                      int* counter,         //!< pointer to a counter for position on line
                                      block_pointer block,  //!< pointer to double to be read
                                      double* parameters)   //!< array of system parameters
{
    SYNCH_PARSE_EXEC;
    // skip the space character, and check the '(' token
    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char argsText[LINELEN];
    strcpy(argsText, line + *counter);
    char* arrEle = strtok(argsText, split);
    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        jaka_value eleValue;
        int subCounter = 0;

        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;

        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);

        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of convey_linear_enable can be only an array variable!"));
            CHKS((eleValue.arrValue.size() != 3), INTERP_ERR_SYNTAX_FAILED, _("The position variable can only have 3 components!"));
            for (size_t i = 0; i < eleValue.arrValue.size(); ++i) block->cmd_pos[i] = eleValue.arrValue[i];
        }
        else if (nArgIndex == 1)
        {  // relative motion or not
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The second argument of movj can be only be a scalar value!"));
            block->conveyor_pulse_equivalent = eleValue.scalarValue;  // zero --> false, non-zero --> true
        }
        else if (nArgIndex == 2)
        {  // for max_distance(optional)
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The 3nd argument of convey_linear_enable can be only be a scalar value!"));
            block->conveyor_max_distance = eleValue.scalarValue;  // zero --> false, non-zero --> true
        }
        ++nArgIndex;
        if (bracketClosed)
            break;
        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 2 && nArgIndex != 3, INTERP_ERR_SYNTAX_FAILED, _(" convey_linear_enable function can only accept 2 or 3 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
    *counter = strlen(line);

    // update the block motion type to JK_MOVL
    block->g_modes[0] = JK_CONVEYOR_LINEAR_ENABLE;
    return 0;
}

int Interp::read_convey_circular_enable(char* line,           //!< string: line of RS274/NGC code being processed
                                        int* counter,         //!< pointer to a counter for position on line
                                        block_pointer block,  //!< pointer to double to be read
                                        double* parameters)   //!< array of system parameters
{
    // skip the space character, and check the '(' token
    SYNCH_PARSE_EXEC;

    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char argsText[LINELEN];
    strcpy(argsText, line + *counter);
    char* arrEle = strtok(argsText, split);
    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        jaka_value eleValue;
        int subCounter = 0;

        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;

        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);

        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of convey_circular_enable can be only an array variable!"));
            CHKS((eleValue.arrValue.size() != 3), INTERP_ERR_SYNTAX_FAILED, _("The position variable can only have 3 components!"));
            for (size_t i = 0; i < eleValue.arrValue.size(); ++i) block->convyr_p1_pos[i] = eleValue.arrValue[i];
        }
        else if (nArgIndex == 1)
        {  // relative motion or not
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of convey_circular_enable can be only an array variable!"));
            CHKS((eleValue.arrValue.size() != 3), INTERP_ERR_SYNTAX_FAILED, _("The position variable can only have 3 components!"));
            for (size_t i = 0; i < eleValue.arrValue.size(); ++i) block->convyr_p2_pos[i] = eleValue.arrValue[i];
        }
        else if (nArgIndex == 2)
        {  // relative motion or not
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of convey_circular_enable can be only an array variable!"));
            CHKS((eleValue.arrValue.size() != 3), INTERP_ERR_SYNTAX_FAILED, _("The position variable can only have 3 components!"));
            for (size_t i = 0; i < eleValue.arrValue.size(); ++i) block->convyr_p3_pos[i] = eleValue.arrValue[i];
        }
        else if (nArgIndex == 3)
        {  // relative motion or not
            CHKS(
                eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The forth argument of convey_circular_enable can be only be a scalar value!"));
            block->conveyor_pulse_equivalent = eleValue.scalarValue * M_PI / 180;  // change degree to rad
        }
        else if (nArgIndex == 4)
        {  // relative motion or not
            CHKS(
                eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The fifth argument of convey_circular_enable can be only be a scalar value!"));
            block->rotate_tool = eleValue.scalarValue;  //
        }
        else if (nArgIndex == 5)
        {  // max distance (optional)
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The 6th argument of convey_circular_enable can be only be a scalar value!"));
            block->conveyor_max_distance = eleValue.scalarValue;
        }
        ++nArgIndex;
        if (bracketClosed)
            break;
        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 5 && nArgIndex != 6, INTERP_ERR_SYNTAX_FAILED, _(" convey_circular_enable function can only accept 5 or 6 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
    *counter = strlen(line);

    // update the block motion type to JK_MOVL
    block->g_modes[0] = JK_CONVEYOR_CIRCULAR_ENABLE;
    rtapi_print("finish read_convey_circular\n");
    return 0;
}

int Interp::read_convey_disable(char* line,           //!< string: line of RS274/NGC code being processed
                                int* counter,         //!< pointer to a counter for position on line
                                block_pointer block,  //!< pointer to double to be read
                                double* parameters)   //!< array of system parameters
{
    //printf("-----------------------------------\n");
    // skip the space character, and check the '(' token
    SYNCH_PARSE_EXEC;
    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;
    //printf("---------------------%c--------------\n",line[*counter]);
    CHKS((line[*counter] != ')'), INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
    *counter = strlen(line);
    // update the block motion type to JK_MOVL
    block->g_modes[0] = JK_CONVEYOR_DISABLE;
    waitFlag = 1;  // interp wait for following relative movement
    return INTERP_WAIT;
}

int Interp::read_atan2(char* line, int* counter, double* double_ptr, double* parameters)
{
    int ret;
    jaka_value arg1, arg2;
    if (double_ptr == nullptr)
    {
        *counter = strlen(line);
        return INTERP_OK;
    }

    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_LEFT_BRACKET_MISSING_AFTER_SLASH_WITH_ATAN);
    *counter = (*counter + 1);

    rtapi_print("read_real_expression: line %s, line[*counter] = %c\n", line, line[*counter]);
    ret = read_real_value(line, counter, &arg1, parameters);
    CHK_INTERP_READ(ret, line, counter);
    CHKS((line[*counter] != ','), INTERP_ERR_SYNTAX_FAILED, NCE_SLASH_MISSING_AFTER_FIRST_ATAN_ARGUMENT);
    *counter = (*counter + 1);

    rtapi_print("read_real_expression: line %s, line[*counter] = %c\n", line, line[*counter]);

    ret = read_real_value(line, counter, &arg2, parameters);
    CHK_INTERP_READ(ret, line, counter);
    CHKS((line[*counter] != ')'), INTERP_ERR_SYNTAX_FAILED, "')' is required to close a function call");
    *counter = (*counter + 1);

    *double_ptr = atan2(arg1.scalarValue, arg2.scalarValue); /* value in radians */
    *double_ptr = ((*double_ptr * 180.0) / M_PI);            /* convert to degrees */
    return INTERP_OK;
}

int Interp::read_getdout(char* line, int* counter, double* double_ptr, double* parameters)
{
    if (!MDImode)
    {
        SYNCH_PARSE_EXEC;
    }
    if (double_ptr == nullptr)
    {
        *counter = strlen(line);
        return INTERP_OK;
    }
    int type = 0, index = 0;
    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);

    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        if (nArgIndex == 0)
        {
            type = atoi(arrEle);
            CHKS((type < 0), INTERP_ERR_SYNTAX_FAILED, _("The type of digital output should be a positive value!"));
        }
        else if (nArgIndex == 1)
        {
            index = atoi(arrEle);
            CHKS((index < 0), INTERP_ERR_SYNTAX_FAILED, _("The index of digital output should be a positive value!"));
        }

        ++nArgIndex;
        if (bracketClosed)
            break;
        arrEle = strtok(NULL, split);
    }

    int status = 0;
    if (usrmotGetDOut(type, index, &status) == -1)
    {
        ERS(CANNOT_ACCESS_EXTIO_SINCE_OFFLINE, _("cannot access extio since it's not running!"));
    }
    *double_ptr = status;
    *counter = strlen(line);

    EXECUTING_BLOCK(_setup).m_modes[11] = GETDOUT;
    return INTERP_OK;
}

int Interp::read_getaout(char* line, int* counter, double* double_ptr, double* parameters)
{
    if (!MDImode)
    {
        SYNCH_PARSE_EXEC;
    }
    if (double_ptr == nullptr)
    {
        *counter = strlen(line);
        return INTERP_OK;
    }
    int type = 0, index = 0;
    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);

    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        if (nArgIndex == 0)
        {
            type = atoi(arrEle);
            CHKS((type < 0), INTERP_ERR_SYNTAX_FAILED, _("The type of analog output should be a positive value!"));
        }
        else if (nArgIndex == 1)
        {
            index = atoi(arrEle);
            CHKS((index < 0), INTERP_ERR_SYNTAX_FAILED, _("The index of analog output should be a positive value!"));
        }

        ++nArgIndex;
        if (bracketClosed)
            break;
        arrEle = strtok(NULL, split);
    }

    if (usrmotGetAOut(type, index, double_ptr) == -1)
    {
        ERS(CANNOT_ACCESS_EXTIO_SINCE_OFFLINE, _("cannot access extio since it's not running!"));
    }
    *counter = strlen(line);

    EXECUTING_BLOCK(_setup).m_modes[11] = GETAOUT;
    return INTERP_OK;
}

int Interp::read_getdin(char* line, int* counter, double* double_ptr, double* parameters)
{
    if (!MDImode)
    {
        SYNCH_PARSE_EXEC;
    }
    if (double_ptr == nullptr)
    {
        *counter = strlen(line);
        return INTERP_OK;
    }
    int type = 0, index = 0;
    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);

    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        if (nArgIndex == 0)
        {
            type = atoi(arrEle);
            //rtapi_print("str is %s, type = %d\n", arrEle, type);
            CHKS((type < 0), INTERP_ERR_SYNTAX_FAILED, _("The type of digital output should be a positive value!"));
        }
        else if (nArgIndex == 1)
        {
            index = atoi(arrEle);
            CHKS((index < 0), INTERP_ERR_SYNTAX_FAILED, _("The index of digital output should be a positive value!"));
        }

        ++nArgIndex;
        if (bracketClosed)
            break;
        arrEle = strtok(NULL, split);
    }
    int status = 0;
    if (usrmotGetDIn(type, index, &status) == -1)
    {
        ERS(CANNOT_ACCESS_EXTIO_SINCE_OFFLINE, _("cannot access extio since it's not running!"));
    }
    *double_ptr = status;
    *counter = strlen(line);

    EXECUTING_BLOCK(_setup).m_modes[11] = GETDIN;
    return INTERP_OK;
}

int Interp::read_getain(char* line, int* counter, double* double_ptr, double* parameters)
{
    if (!MDImode)
    {
        SYNCH_PARSE_EXEC;
    }
    if (double_ptr == nullptr)
    {
        *counter = strlen(line);
        return INTERP_OK;
    }
    int type = 0, index = 0;
    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);

    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        if (nArgIndex == 0)
        {
            type = atoi(arrEle);
            CHKS((type < 0), INTERP_ERR_SYNTAX_FAILED, _("The type of digital output should be a positive value!"));
        }
        else if (nArgIndex == 1)
        {
            index = atoi(arrEle);
            CHKS((index < 0), INTERP_ERR_SYNTAX_FAILED, _("The index of digital output should be a positive value!"));
        }

        ++nArgIndex;
        if (bracketClosed)
            break;
        arrEle = strtok(NULL, split);
    }

    if (usrmotGetAIn(type, index, double_ptr) == -1)
    {
        ERS(CANNOT_ACCESS_EXTIO_SINCE_OFFLINE, _("cannot access extio since it's not running!"));
    }
    *counter = strlen(line);

    EXECUTING_BLOCK(_setup).m_modes[11] = GETAIN;
    return INTERP_OK;
}

int Interp::read_getatljointpose(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    SYNCH_PARSE_EXEC;
    if (value_ptr == nullptr)
    {
        *counter = strlen(line);
        return INTERP_OK;
    }
    value_ptr->valueType = ARRAY_VALUE;
    value_ptr->arrValue.resize(6);
    double jointPos[6] = {0};
    if (usrmotGetAtlJointPose(jointPos) == 0)
    {
        for (int i = 0; i < 6; i++)
        {
            value_ptr->arrValue[i] = jointPos[i];
            // rtapi_print("[read_getatljointpose]-->value_ptr->arrValue[%d] = %f\n", i, value_ptr->arrValue[i]);
        }
    }

    *counter = strlen(line);
    return INTERP_OK;
}

int Interp::read_getatltcppose(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    SYNCH_PARSE_EXEC;
    if (value_ptr == nullptr)
    {
        *counter = strlen(line);
        return INTERP_OK;
    }
    value_ptr->valueType = ARRAY_VALUE;
    value_ptr->arrValue.resize(6);
    double tcpPos[6] = {0};
    if (usrmotGetAtlTcpPose(tcpPos) == 0)
    {
        for (int i = 0; i < 6; i++) { value_ptr->arrValue[i] = tcpPos[i]; }
    }
    *counter = strlen(line);
    return INTERP_OK;
}

int Interp::read_getatlflangepose(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    SYNCH_PARSE_EXEC;
    if (value_ptr == nullptr)
    {
        *counter = strlen(line);
        return INTERP_OK;
    }
    value_ptr->valueType = ARRAY_VALUE;
    value_ptr->arrValue.resize(6);
    double flangePos[6] = {0};
    if (usrmotGetAtlFlangePose(flangePos) == 0)
    {
        for (int i = 0; i < 6; i++)
        {
            value_ptr->arrValue[i] = flangePos[i];
            // rtapi_print("value_ptr->arrValue[%d] = %f\n", i, value_ptr->arrValue[i]);
        }
    }
    *counter = strlen(line);
    return INTERP_OK;
}

/* for getting the tcp offsets */
int Interp::read_gettooloffsetsof(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    SYNCH_PARSE_EXEC;
    if (value_ptr == nullptr)
    {
        *counter = strlen(line);
        return INTERP_OK;
    }
    value_ptr->valueType = ARRAY_VALUE;
    value_ptr->arrValue.resize(6);
    ZucPose tcp_offsets;
    int id = 0;

    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);

    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        if (nArgIndex == 0)
        {
            id = atoi(arrEle);
            CHKS(((id < 0) || (id >= 16)), INTERP_ERR_SYNTAX_FAILED, _("The type of digital output should be a positive value that less than 16!"));
        }

        ++nArgIndex;
        if (bracketClosed)
            break;
        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 1, INTERP_ERR_SYNTAX_FAILED, _("gettooloffsetsof can only accept 1 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));

    GET_TOOL_OFFSET(id, &tcp_offsets);
    value_ptr->arrValue[0] = tcp_offsets.tran.x;
    value_ptr->arrValue[1] = tcp_offsets.tran.y;
    value_ptr->arrValue[2] = tcp_offsets.tran.z;
    value_ptr->arrValue[3] = tcp_offsets.a;
    value_ptr->arrValue[4] = tcp_offsets.b;
    value_ptr->arrValue[5] = tcp_offsets.c;

    *counter = strlen(line);
    return INTERP_OK;
}

int Interp::read_gettooloffsets(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    SYNCH_PARSE_EXEC;
    if (value_ptr == nullptr)
    {
        *counter = strlen(line);
        return INTERP_OK;
    }
    value_ptr->valueType = ARRAY_VALUE;
    value_ptr->arrValue.resize(6);
    double tcpOffsets[6] = {0};
    if (usrmotGetToolOffsets(tcpOffsets) == 0)
    {
        for (int i = 0; i < 6; i++) { value_ptr->arrValue[i] = tcpOffsets[i]; }
    }

    *counter = strlen(line);
    return INTERP_OK;
}

/* for getting the user offsets */
int Interp::read_getuseroffsetsof(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    SYNCH_PARSE_EXEC;
    if (value_ptr == nullptr)
    {
        *counter = strlen(line);
        return INTERP_OK;
    }
    value_ptr->valueType = ARRAY_VALUE;
    value_ptr->arrValue.resize(6);
    ZucPose user_offsets;
    int id = 0;

    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);

    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        if (nArgIndex == 0)
        {
            id = atoi(arrEle);
            CHKS(((id < 0) || (id >= 16)), INTERP_ERR_SYNTAX_FAILED, _("The type of digital output should be a positive value that less than 16!"));
        }

        ++nArgIndex;
        if (bracketClosed)
            break;
        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 1, INTERP_ERR_SYNTAX_FAILED, _("gettooloffsetsof can only accept 1 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));

    GET_USER_OFFSET(id, &user_offsets);

    value_ptr->arrValue[0] = user_offsets.tran.x;
    value_ptr->arrValue[1] = user_offsets.tran.y;
    value_ptr->arrValue[2] = user_offsets.tran.z;
    value_ptr->arrValue[3] = user_offsets.a;
    value_ptr->arrValue[4] = user_offsets.b;
    value_ptr->arrValue[5] = user_offsets.c;

    *counter = strlen(line);
    return INTERP_OK;
}

int Interp::read_getuseroffsets(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    SYNCH_PARSE_EXEC;
    if (value_ptr == nullptr)
    {
        *counter = strlen(line);
        return INTERP_OK;
    }
    value_ptr->valueType = ARRAY_VALUE;
    value_ptr->arrValue.resize(6);
    double userOffsets[6] = {0};
    if (usrmotGetUserOffsets(userOffsets) == 0)
    {
        for (int i = 0; i < 6; i++)
        {
            value_ptr->arrValue[i] = userOffsets[i];
            // rtapi_print("[read_getatljointpose]-->value_ptr->arrValue[%d] = %f\n", i, value_ptr->arrValue[i]);
        }
    }

    *counter = strlen(line);
    return INTERP_OK;
}

int Interp::read_getclsnsensitivity(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    SYNCH_PARSE_EXEC;
    *counter = strlen(line);
    if (value_ptr == nullptr)
    {
        *counter = strlen(line);
        return INTERP_OK;
    }

    value_ptr->valueType = SCALAR_VALUE;
    int clsn_sensitivity;
    if (usrmotGetClsnSensitivity(clsn_sensitivity) == 0)
    {
        value_ptr->scalarValue = clsn_sensitivity;
        // rtapi_print("*****[read_getclsnsensitivity]*****-->value_ptr->scalarValue = %f\n", value_ptr->scalarValue);
    }

    *counter = strlen(line);
    return INTERP_OK;
}

int Interp::read_getpayload(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    SYNCH_PARSE_EXEC;
    if (value_ptr == nullptr)
    {
        *counter = strlen(line);
        return INTERP_OK;
    }

    value_ptr->valueType = ARRAY_VALUE;
    value_ptr->arrValue.resize(4);
    double payload[4] = {0};
    if (usrmotGetToolPayload(payload) == 0)
    {
        for (int i = 0; i < 4; i++)
        {
            value_ptr->arrValue[i] = payload[i];
            // rtapi_print("[read_getpayload]-->value_ptr->arrValue[%d] = %f\n", i, value_ptr->arrValue[i]);
        }
    }

    *counter = strlen(line);
    return INTERP_OK;
}

/* for getting sensor values */
int Interp::read_getsensortorque(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    SYNCH_PARSE_EXEC;
    if (value_ptr == nullptr)
    {
        *counter = strlen(line);
        return INTERP_OK;
    }
    value_ptr->valueType = ARRAY_VALUE;
    value_ptr->arrValue.resize(6);
    double sensor_value[6] = {0};
    int sensor_type = 0;
    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);

    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        if (nArgIndex == 0)
        {
            sensor_type = atoi(arrEle);
            CHKS(((sensor_type != 0) && (sensor_type != 1)),
                 INTERP_ERR_SYNTAX_FAILED,
                 _("Sensor type must be 0 or 1, where 0 for end force sensor, and 1 for base force sensor!\n"));
        }

        ++nArgIndex;
        if (bracketClosed)
            break;
        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 1, INTERP_ERR_SYNTAX_FAILED, _("gettooloffsetsof can only accept 1 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));

    if (usrmotGetSensorType(sensor_type, sensor_value) == 0)
    {
        for (int i = 0; i < 6; i++)
        {
            value_ptr->arrValue[i] = sensor_value[i];
            // rtapi_print(" *** *** *** *** value_ptr[%d] is %f\n", i, value_ptr->arrValue[i]);
        }
    }

    *counter = strlen(line);
    return INTERP_OK;
}

int Interp::read_gettimeout(char* line, int* counter, double* double_ptr, double* parameters)
{
    SYNCH_PARSE_EXEC;
    if (double_ptr == nullptr)
    {
        *counter = strlen(line);
        return INTERP_OK;
    }
    if (waitTimeout)
    {
        *double_ptr = 1.0;
    }
    else
    {
        *double_ptr = 0.0;
    }

    *counter = strlen(line);
    return INTERP_OK;
}

// int Interp::read_getarrayelement(char* line, int* counter, jaka_value* value_ptr, double* parameters)
// {
// 	int ret = INTERP_OK;
// 	value_ptr->valueType = SCALAR_VALUE;
// 	value_ptr->scalarValue = 0;

// 	if (!parsing_flag) {
// 		jaka_value arr_val;
// 		int eleIndex = 0;

// 		while (*counter < (int)strlen(line) && line[*counter] == ' ')
// 			*counter = *counter + 1;
// 		CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
// 		*counter = *counter + 1;

// 		char argsText[LINELEN];
// 		strcpy(argsText, line + *counter);

// 		int nArgIndex = 0;
// 		bool bracketClosed = false;
// 		const char* split = ",";
// 		char* arrEle = strtok(argsText, split);
// 		while (arrEle != NULL) {
// 			char* pchar = strstr(arrEle, ")");
// 			if (pchar != NULL) { // contain the ')'
// 				bracketClosed = true;
// 				*pchar = '\0';
// 			}

// 			jaka_value eleValue;
// 			int subCounter = 0;
// 			while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ') // skip the space
// 				++subCounter;

// 			ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
// 			CHK_INTERP_READ(ret, line, counter);
// 			if (nArgIndex == 0) {
// 				CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_INVALID_ARRAY, _("The first argument of getarrayelement can be only an array value!"));
// 				arr_val = eleValue;
// 			} else if (nArgIndex == 1) {
// 				CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_INVALID_INDEX, _("The second argument of getarrayelement can be only an integer value!"));
// 				CHKS(eleValue.scalarValue >= arr_val.arrValue.size(), INTERP_ERR_INVALID_INDEX, _("Array index out of range!"));
// 				eleIndex = eleValue.scalarValue;
// 			}

// 			++nArgIndex;
// 			if (bracketClosed) break;
// 			arrEle = strtok(NULL, split);
// 		}

// 		CHKS(nArgIndex!=2, INTERP_ERR_SYNTAX_FAILED, _("getarrayelement can only accept 2 arguments!"));
// 		CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));

// 		value_ptr->valueType = SCALAR_VALUE;
// 		value_ptr->scalarValue = arr_val.arrValue[eleIndex];
// 		// rtapi_print("the %dth element of array is: %f\n", eleIndex, value_ptr->scalarValue);
// 	}

// 	*counter = strlen(line);
// 	return INTERP_OK;
// }

// int Interp::read_setarrayelement(char *line, int *counter, double *parameters)
// {
// 	// setArrayElement(#<arr>, #<idx>, #<val>)
// 	int ret = INTERP_OK;
// 	if (!parsing_flag) {
// 		jaka_value array;
// 		char arrName[256];
// 		int eleIndex = 0;
// 		double element;

// 		while (*counter < (int)strlen(line) && line[*counter] == ' ')
// 			*counter = *counter + 1;
// 		CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
// 		*counter = *counter + 1;

// 		char argsText[LINELEN];
// 		strcpy(argsText, line + *counter);

// 		int nArgIndex = 0;
// 		bool bracketClosed = false;
// 		const char* split = ",";
// 		char* arrEle = strtok(argsText, split);
// 		while (arrEle != NULL) {
// 			char* pchar = strstr(arrEle, ")");
// 			if (pchar != NULL) { // contain the ')'
// 				bracketClosed = true;
// 				*pchar = '\0';
// 			}

// 			jaka_value eleValue;
// 			int subCounter = 0;
// 			while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ') // skip the space
// 				++subCounter;

// 			ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
// 			CHK_INTERP_READ(ret, line, counter);
// 			if (nArgIndex == 0) {
// 				CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of setarrayelement can be only an array value!"));
// 				array = eleValue;
// 				subCounter = 0;
// 				read_name(arrEle, &subCounter, arrName);
// 			} else if (nArgIndex == 1) {
// 				CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The second argument of setarrayelement can be only an integer value!"));
// 				CHKS(eleValue.scalarValue >= array.arrValue.size(), INTERP_ERR_SYNTAX_FAILED, _("Array index out of range!"));
// 				eleIndex = round_to_int(eleValue.scalarValue);
// 			} else if (nArgIndex == 2) {
// 				CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The third argument of setarrayelement can be only a scalar value!"));
// 				element = eleValue.scalarValue;
// 			}

// 			++nArgIndex;
// 			if (bracketClosed) break;
// 			arrEle = strtok(NULL, split);
// 		}

// 		CHKS(nArgIndex!=3, INTERP_ERR_SYNTAX_FAILED, _("setarrayelement can only accept 3 arguments!"));
// 		CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));

// 		const char *dup;
// 		dup = strstore(arrName);
// 		array.arrValue[eleIndex] = element;
// 		CHP(store_named_param(&_setup, dup, array));
// 		// rtapi_print("Array element set: %d %f\n", eleIndex, element);
// 	}

// 	*counter = strlen(line);
// 	return INTERP_OK;
// }

int Interp::read_getlength(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    int ret = INTERP_OK;
    if (value_ptr)
    {
        value_ptr->valueType = SCALAR_VALUE;
        value_ptr->scalarValue = 0;
    }
    if (!parsing_flag)
    {
        jaka_value var_val;
        while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
        CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
        *counter = *counter + 1;
        char argsText[LINELEN];
        strcpy(argsText, line + *counter);

        int nArgIndex = 0;
        bool bracketClosed = false;
        const char* split = ",";
        char* arrEle = strtok(argsText, split);
        while (arrEle != NULL)
        {
            char* pchar = strstr(arrEle, ")");
            if (pchar != NULL)
            {  // contain the ')'
                bracketClosed = true;
                *pchar = '\0';
            }

            jaka_value eleValue;
            int subCounter = 0;
            while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
                ++subCounter;

            ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
            CHK_INTERP_READ(ret, line, counter);
            if (nArgIndex == 0)
            {
                // CHKS((eleValue.valueType != ARRAY_VALUE && eleValue.valueType != STRING_VALUE),
                //      INTERP_ERR_INVALID_ARRAY,
                //      _("The first argument of getsize can be only an array or string value!"));
                var_val = eleValue;
            }
            ++nArgIndex;
            if (bracketClosed)
                break;
            arrEle = strtok(NULL, split);
        }

        CHKS(nArgIndex != 1, INTERP_ERR_SYNTAX_FAILED, _("getlength can only accept 1 arguments!"));
        CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));

        if (value_ptr)
        {
            if (var_val.valueType == ARRAY_VALUE)
            {
                value_ptr->scalarValue = var_val.arrValue.size();
            }
            else if (var_val.valueType == SCALAR_VALUE)
            {
                value_ptr->valueType = SCALAR_VALUE;
                value_ptr->scalarValue = -1;
            }
            else
                value_ptr->scalarValue = var_val.strValue.size();
        }
    }

    *counter = strlen(line);
    return INTERP_OK;
}

int Interp::read_waitinput(char* line, int* counter, block_pointer block, double* parameters)
{
    if (_setup.defining_sub)
        return INTERP_OK;

    SYNCH_PARSE_EXEC;

    if (beforeProgStartLine)
    {
        *counter = strlen(line);
        return INTERP_OK;
    }

    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);
    // rtapi_print("read_waitinput: argsText is %s\n", argsText);

    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        jaka_value eleValue;
        int subCounter = 0;

        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;

        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);

        if (nArgIndex == 0)
        {  // input type
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The 1st argument of waitinput can be only a scalar value!"));
            CHKS((eleValue.scalarValue < 0.0), INTERP_ERR_SYNTAX_FAILED, _("The input index cannot be a negative value!"));
            block->wait_di_type = round_to_int(eleValue.scalarValue);
            // rtapi_print("read_waitinput: the 1st argument retrieved!\n");
        }
        else if (nArgIndex == 1)
        {  // input index
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The 2nd argument of waitinput can be only a scalar value!"));
            CHKS((eleValue.scalarValue < 0.0), INTERP_ERR_SYNTAX_FAILED, _("The input index cannot be a negative value!"));
            block->wait_di_index = round_to_int(eleValue.scalarValue);
            // rtapi_print("read_waitinput: the 2nd argument retrieved!\n");
        }
        else if (nArgIndex == 2)
        {  // trigger type
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The 3rd argument of waitinput can be only a scalar value!"));
            CHKS((eleValue.scalarValue < 0.0), INTERP_ERR_SYNTAX_FAILED, _("The wait mode cannot be a negative value!"));
            if (round_to_int(eleValue.scalarValue) == 0)
            {
                block->wait_di_stat = 4;  // low
            }
            else
            {
                block->wait_di_stat = 3;  // high
            }
            // rtapi_print("read_waitinput: the 3rd argument retrieved!\n");
        }
        else if (nArgIndex == 3)
        {  // timeout in seconds for waiting
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The 4th argument of waitinput can be only a scalar value!"));
            CHKS((eleValue.scalarValue < 0.0), INTERP_ERR_SYNTAX_FAILED, _("The tolerance cannot be a negative value!"));
            if (eleValue.scalarValue < 1e-8)
            {
                block->wait_di_timeout = 10000000000.0;
            }
            else
            {
                block->wait_di_timeout = eleValue.scalarValue;
            }
            // rtapi_print("read_waitinput: the 4th argument retrieved!\n");
        }

        ++nArgIndex;
        if (bracketClosed)
            break;

        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 4, INTERP_ERR_SYNTAX_FAILED, _("waitinput function can only accept 3 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
    *counter = strlen(line);

    // update the block motion type to waitiput
    block->m_modes[5] = JK_WAITINPUT;
    block->m_count++;
    block->from_thread = m_threadId;

    return INTERP_OK;
}

int Interp::read_set_planner_type(char* line, int* counter)
{
    std::string str = line;
    str = str.substr(*counter, str.size() - *counter);
    try
    {
        boost::tokenizer<boost::char_separator<char>> tok(str, boost::char_separator<char>(" ,()[]", ""));
        for (auto& x : tok)
        {
            std::stringstream mid;
            mid << x;
            double res;
            mid >> std::dec >> res;
            set_planner_type = res;
        }
    }
    catch (std::exception& e)
    {
        rtapi_print("%s\n", e.what());
    }
    *counter = strlen(line);
    return INTERP_OK;
}

int Interp::read_close_speed_planner(char* line, int* counter)
{
    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;
    //printf("---------------------%c--------------\n",line[*counter]);
    CHKS((line[*counter] != ')'), INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
    *counter = strlen(line);
    // update the block motion type to JK_MOVL
    //block->g_modes[0] = JK_CLOSE_SPEED_PLANNER;
    set_planner_type = -1;
    return INTERP_OK;
}

int Interp::read_override_speed_par(char* line, int* counter)
{
    std::string str = line;
    str = str.substr(*counter, str.size() - *counter);
    int i = 0, typeJudge = -1;
    double vel = 0.0, acc = 0.0;
    double ori_vel = 180.0, ori_acc = 720.0;  //set orientation vel/acc with max value and limit after;
    try
    {
        boost::tokenizer<boost::char_separator<char>> tok(str, boost::char_separator<char>(" ,()[]", ""));
        for (auto& x : tok)
        {
            std::stringstream mid;
            mid << x;
            double res;
            mid >> std::dec >> res;
            switch (i)
            {
            case 0:
                if (int(res) == 0)
                {
                    speed_info_movl.speed_override_symbol = 1;
                    typeJudge = 0;
                }
                if (int(res) == 1)
                {
                    speed_info_movj.speed_override_symbol = 1;
                    typeJudge = 1;
                }
                break;
            case 1:
                vel = res;
                break;
            case 2:
                acc = res;
                break;
            case 3:
                ori_vel = res;
                break;
            case 4:
                ori_acc = res;
                break;
            default:
                break;
            }
            ++i;
        }
    }
    catch (std::exception& e)
    {
        rtapi_print("%s\n", e.what());
    }
    if (typeJudge)
    {
        speed_info_movj.vel = vel;
        speed_info_movj.acc = acc;
    }
    else
    {
        speed_info_movl.vel = vel;
        speed_info_movl.acc = acc;
        speed_info_movl.ori_vel = ori_vel;  //orientation velocity and acceleration
        speed_info_movl.ori_acc = ori_acc;
    }
    *counter = strlen(line);
    return INTERP_OK;
}

int Interp::read_stop_override_speed(char* line, int* counter)
{
    std::string str = line;
    str = str.substr(*counter, str.size() - *counter);
    try
    {
        boost::tokenizer<boost::char_separator<char>> tok(str, boost::char_separator<char>(" ,()[]", ""));
        for (auto& x : tok)
        {
            std::stringstream mid;
            mid << x;
            double res;
            mid >> std::dec >> res;
            if (int(res) == 0)
                speed_info_movl.speed_override_symbol = 0;
            if (int(res) == 1)
                speed_info_movj.speed_override_symbol = 0;
        }
    }
    catch (std::exception& e)
    {
        rtapi_print("%s\n", e.what());
    }
    *counter = strlen(line);
    return INTERP_OK;
}

int Interp::read_socketopen(char* line, int* counter, int* int_ptr, double* parameters)
{
    SYNCH_PARSE_EXEC;

    int ret = INTERP_OK;
    int sockid = -1;
    if (!parsing_flag)
    {
        std::string svr_ip;
        int svr_port = 23;
        /*parse the arguments*/
        while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
        CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
        *counter = *counter + 1;

        char argsText[LINELEN];
        strcpy(argsText, line + *counter);

        int nArgIndex = 0;
        bool bracketClosed = false;
        const char* split = ",";
        char* arrEle = strtok(argsText, split);
        while (arrEle != NULL)
        {
            char* pchar = strstr(arrEle, ")");
            if (pchar != NULL)
            {  // contain the ')'
                bracketClosed = true;
                *pchar = '\0';
            }

            jaka_value eleValue;
            int subCounter = 0;
            while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
                ++subCounter;

            ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
            CHK_INTERP_READ(ret, line, counter);
            if (nArgIndex == 0)
            {
                CHKS(eleValue.valueType != STRING_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of socketopen can be only a string value!"));
                svr_ip = eleValue.strValue;
                rtapi_print("socketopen: the first argument is %s\n", svr_ip.c_str());
            }
            else if (nArgIndex == 1)
            {
                CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The second argument of socketopen can be only an integer value!"));
                svr_port = (int)(eleValue.scalarValue);
                rtapi_print("socketopen: the second argument is %d\n", svr_port);
            }

            ++nArgIndex;
            if (bracketClosed)
                break;

            arrEle = strtok(NULL, split);
        }

        CHKS(nArgIndex != 2, INTERP_ERR_SYNTAX_FAILED, _("socketopen function can only accept 2 arguments!"));
        CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));

        // int sentsize = 0, recvsize = 0;
        struct sockaddr_in svr_addr;
        sockid = socket(AF_INET, SOCK_STREAM | SOCK_NONBLOCK, 0);
        if (sockid <= 0)
        {
            if (int_ptr)
                *int_ptr = -1;  // return socket id
            *counter = strlen(line);
            return INTERP_OK;
        }
        else
        {
            if (sock_list.size() >= 256)
                ERS(INTERP_ERR_TOO_MANY_SOCKET, "too many unclosed sockets");
            /*record the socket id*/
            sock_list.push_back(sockid);
        }
        memset(&svr_addr, 0, sizeof(svr_addr));
        svr_addr.sin_family = AF_INET;
        svr_addr.sin_addr.s_addr = inet_addr(svr_ip.c_str());
        svr_addr.sin_port = htons(svr_port);

        /* use non-block mode when connecting*/
        int connRes = -1;
        for (int i = 0; i < 5; i++)
        {
            connRes = connect(sockid, (struct sockaddr*)&svr_addr, sizeof(svr_addr));
            if (connRes == 0)
            {
                break;
            }
            if (m_isSteppingMode == 1)  // 单步调试时如果等待的时间过长，连续点单步时APP会超时掉线，如果不等待会偶出现连接不上server的情况。
                esleep(0.03);
            else
                esleep(0.3);
        }
        if (connRes == -1)
        {
            rtapi_print("connection failed, errno is %d\n", errno);
            if (int_ptr)
                *int_ptr = -1;
            *counter = strlen(line);

            ::close(sockid);
            sock_list.erase(remove(sock_list.begin(), sock_list.end(), sockid), sock_list.end());
            // ERS(INTERP_ERR_SOCKET_CREATE, "faild to create the socket %s:%d\n",svr_ip.data(),svr_port);
            return INTERP_OK;
        }
        rtapi_print("server addr is %s, port is %d\n", svr_ip.c_str(), svr_port);
    }

    if (int_ptr)
        *int_ptr = sockid;  // return socket id
    *counter = strlen(line);

    EXECUTING_BLOCK(_setup).m_modes[11] = SOCKETOPEN;
    return ret;
}

int Interp::read_socketrecv(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    if (!MDImode)
    {
        SYNCH_PARSE_EXEC;
    }
    if (value_ptr)
    {
        value_ptr->valueType = STRING_VALUE;
        value_ptr->strValue = std::string("");
    }
    int ret = INTERP_OK;
    if (!parsing_flag && !beforeProgStartLine)
    {
        int sockid = -1;
        double timeout = 2.0;

        while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
        CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
        *counter = *counter + 1;

        char argsText[LINELEN];
        strcpy(argsText, line + *counter);

        int nArgIndex = 0;
        bool bracketClosed = false;
        const char* split = ",";
        char* arrEle = strtok(argsText, split);
        while (arrEle != NULL)
        {
            char* pchar = strstr(arrEle, ")");
            if (pchar != NULL)
            {  // contain the ')'
                bracketClosed = true;
                *pchar = '\0';
            }

            jaka_value eleValue;
            int subCounter = 0;
            while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
                ++subCounter;

            ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
            CHK_INTERP_READ(ret, line, counter);
            if (nArgIndex == 0)
            {
                CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of socketread can be only an integer value!"));
                sockid = round_to_int(eleValue.scalarValue);
            }
            else if (nArgIndex == 1)
            {
                CHKS(eleValue.valueType != SCALAR_VALUE || (round_to_int(eleValue.scalarValue) < 0),
                     INTERP_ERR_SYNTAX_FAILED,
                     _("The forth argument of socketreadstring can be only a non-negative real value!"));
                timeout = fabs(eleValue.scalarValue) < 10e-6 ? 65536 : eleValue.scalarValue;
            }

            ++nArgIndex;
            if (bracketClosed)
                break;
            arrEle = strtok(NULL, split);
        }

        CHKS(nArgIndex != 2, INTERP_ERR_SYNTAX_FAILED, _("socketread function can only accept 2 arguments!"));
        CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));

        if (sockid < 0)
        {
            if (value_ptr)
            {
                value_ptr->valueType = SCALAR_VALUE;
                value_ptr->scalarValue = -1;
            }
            *counter = strlen(line);
            return INTERP_OK;
        }

        if (start_time == 0)
        {
            start_time = etime();
        }

        memset(cam_recv_buf, 0, sizeof(cam_recv_buf));
        int recvsize = recv(sockid, cam_recv_buf, sizeof(cam_recv_buf), 0);
        if (recvsize > 0)
        {  // data received
            if (value_ptr)
            {
                value_ptr->strValue = std::string(cam_recv_buf);
                rtapi_print("variable get successfully: %s\n\n", value_ptr->strValue.c_str());
            }
        }
        else if (recvsize == 0)
        {  // socket closed by server side
            if (value_ptr)
            {
                value_ptr->valueType = SCALAR_VALUE;
                value_ptr->scalarValue = -1;
            }
            *counter = strlen(line);
            ERS(INTERP_ERR_SOCKET_CONNECT, "socket closed by server side");
        }
        else if (recvsize < 0 && (errno != EINTR && errno != EWOULDBLOCK && errno != EAGAIN))
        {
            if (value_ptr)
            {  // abnomal receive
                value_ptr->valueType = SCALAR_VALUE;
                value_ptr->scalarValue = -1;
            }
        }
        else
        {  // normal receive
            if (etime() - start_time < timeout)
            {
                esleep(0.1);
                WAIT_PARSE_EXEC;
            }
            else
            {  // receive timeout
                start_time = 0;
                *counter = strlen(line);
                rtapi_print("socket_recv() timeout. \n");
                return INTERP_OK;
            }
        }
    }

    start_time = 0;
    *counter = strlen(line);

    EXECUTING_BLOCK(_setup).m_modes[11] = SOCKETRECV;
    return INTERP_OK;
}

int Interp::read_socketgetvar(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    if (!MDImode)
    {
        SYNCH_PARSE_EXEC;
    }
    if (value_ptr)
    {
        value_ptr->valueType = SCALAR_VALUE;
        value_ptr->scalarValue = FLT_MAX;
    }

    int ret = INTERP_OK;
    if (!parsing_flag && !beforeProgStartLine)
    {
        int sockid = -1;
        double timeout = 2.0;
        int vartype = 0;  // 0-->int, 1-->float, 2-->string
        std::string varname;

        while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
        CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
        *counter = *counter + 1;

        char argsText[LINELEN];
        strcpy(argsText, line + *counter);
        rtapi_print("socketgetvar:argsText is %s\n", argsText);
        int nArgIndex = 0;
        bool bracketClosed = false;
        const char* split = ",";
        char* arrEle = strtok(argsText, split);
        while (arrEle != NULL)
        {
            char* pchar = strstr(arrEle, ")");
            if (pchar != NULL)
            {  // contain the ')'
                bracketClosed = true;
                *pchar = '\0';
            }

            jaka_value eleValue;
            int subCounter = 0;
            while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
                ++subCounter;

            ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
            CHK_INTERP_READ(ret, line, counter);
            if (nArgIndex == 0)
            {
                CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of socketgetvar can be only an integer value!"));
                sockid = round_to_int(eleValue.scalarValue);
                rtapi_print("socketgetvar: the first argument is %d\n", sockid);
            }
            else if (nArgIndex == 1)
            {
                CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The second argument of socketgetvar can be only an integer value!"));
                vartype = round_to_int(eleValue.scalarValue);
                rtapi_print("socketgetvar: the second argument is %d\n", vartype);
            }
            else if (nArgIndex == 2)
            {
                CHKS(eleValue.valueType != STRING_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The third argument of socketgetvar can be only a string value!"));
                varname = eleValue.strValue;
                rtapi_print("socketgetvar: the third argument is %s\n", varname.c_str());
            }
            else if (nArgIndex == 3)
            {
                CHKS(eleValue.valueType != SCALAR_VALUE || (round_to_int(eleValue.scalarValue) < 0),
                     INTERP_ERR_SYNTAX_FAILED,
                     _("The third argument of socketgetvar can be only a non-negative real value!"));
                timeout = fabs(eleValue.scalarValue) < 10e-6 ? 65536 : eleValue.scalarValue;
            }

            ++nArgIndex;
            if (bracketClosed)
                break;
            arrEle = strtok(NULL, split);
        }

        CHKS(nArgIndex != 3 && nArgIndex != 4, INTERP_ERR_SYNTAX_FAILED, _("socketgetvar function can only accept 3 or 4 arguments!"));
        CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));

        int sentsize, recvsize;
        if (sockid < 0)
        {
            if (value_ptr)
            {
                value_ptr->valueType = SCALAR_VALUE;
                value_ptr->scalarValue = -1;
            }
            *counter = strlen(line);
            return INTERP_OK;
        }

        if (start_time == 0)
        {
            std::string cmdtosend = "get <" + varname + ">";
            sentsize = send(sockid, cmdtosend.c_str(), cmdtosend.size(), 0);
            if (sentsize < 0)
            {
                if (value_ptr)
                {
                    value_ptr->valueType = SCALAR_VALUE;
                    value_ptr->scalarValue = -1;
                }
                *counter = strlen(line);
                return INTERP_OK;
            }
            start_time = etime();
        }
        memset(cam_recv_buf, 0, sizeof(cam_recv_buf));
        recvsize = recv(sockid, cam_recv_buf, sizeof(cam_recv_buf), 0);
        rtapi_print("recvsize  %d \n", recvsize);
        if (recvsize > 0)
        {
            rtapi_print("succeed to receive data: %s, size %d\n", cam_recv_buf, recvsize);
            std::string s(cam_recv_buf);
            trim(s);
            std::regex reg("<(.*)><(.*)>");
            std::smatch sm;
            rtapi_print("varname0 ....:%s \n", varname.c_str());

            if (std::regex_match(s, sm, reg) && sm[1].str() == varname)
            {
                jaka_value eleValue;
                int subCounter = 0;
                std::string valuestring = sm[2].str();
                valuestring.erase(0, valuestring.find_first_not_of(" "));  // remove the space in the head
                strcpy(argsText, valuestring.c_str());
                ret = read_real_value(argsText, &subCounter, &eleValue, parameters);
                CHK_INTERP_READ(ret, line, counter);
                if (value_ptr)
                {
                    *value_ptr = eleValue;
                    rtapi_print("value_ptr->valueType ....:%d \n", value_ptr->valueType);
                    rtapi_print("value_ptr->strValue ....:%s \n", value_ptr->strValue.c_str());
                }
            }
            else
            {
                if (value_ptr)
                {  // abnomal receive
                    value_ptr->valueType = SCALAR_VALUE;
                    value_ptr->scalarValue = -1;
                }
                rtapi_print("socket_getvar() recv ERROR. Expect recv data with format '<varName><varScalar>' or '<varName><\"varString\">' or "
                            "'<varName><[varArray]>'\n");
                ERS(INTERP_WARNING_SOCKET_RECV_INVALID_DATA,
                    "socketgetvar()  ERROR FORMAT.  Expect recv data with format '<varName><varScalar>' or '<varName><\"varString\">' or "
                    "'<varName><[varArray]>'");
            }
        }
        else if (recvsize == 0)
        {  // socket closed by server side
            *counter = strlen(line);
            ERS(INTERP_ERR_SOCKET_CONNECT, "socket closed by server side");
        }
        else if (recvsize < 0 && (errno != EINTR && errno != EWOULDBLOCK && errno != EAGAIN))
        {
            // suppose it to be connection error
            if (value_ptr)
            {  // abnomal receive
                value_ptr->valueType = SCALAR_VALUE;
                value_ptr->scalarValue = -1;
            }
        }
        else
        {  // normal receive
            if (etime() - start_time < timeout)
            {
                esleep(0.1);
                WAIT_PARSE_EXEC;
            }
            else
            {  // receive timeout
                start_time = 0;
                *counter = strlen(line);
                rtapi_print("socket_getvar() timeout. \n");
                return INTERP_OK;
            }
        }
    }

    start_time = 0;
    *counter = strlen(line);

    EXECUTING_BLOCK(_setup).m_modes[11] = SOCKETGETVAR;
    return INTERP_OK;
}

int Interp::read_socketclose(char* line, int* counter, double* parameters)
{
    SYNCH_PARSE_EXEC;
    rtapi_print("read_socketclose\n");
    int res = INTERP_OK;
    int sockid = -1;
    if (!parsing_flag)
    {
        /*parse the arguments*/
        while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
        CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
        *counter = *counter + 1;
        char argsText[LINELEN];
        strcpy(argsText, line + *counter);

        int nArgIndex = 0;
        bool bracketClosed = false;
        const char* split = ",";
        char* arrEle = strtok(argsText, split);
        char* sock_id_param_name;
        int exists = 0;
        jaka_value sock_id_param_val;
        while (arrEle != NULL)
        {
            char* pchar = strstr(arrEle, ")");
            if (pchar != NULL)
            {  // contain the ')'
                bracketClosed = true;
                *pchar = '\0';
            }

            jaka_value eleValue;
            int subCounter = 0;
            while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
                ++subCounter;

            if (nArgIndex == 0)
            {
                int cnt = subCounter;
                CHP(read_named_parameter_setting(arrEle, &cnt, &sock_id_param_name, parameters));
                CHP(find_named_param(sock_id_param_name, &exists, &sock_id_param_val));
                rtapi_print("sock_ID_param: %s \n", sock_id_param_name);

                res = read_real_value(arrEle, &subCounter, &eleValue, parameters);
                CHK_INTERP_READ(res, line, counter);
                CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of sockclose can be only an integer value!"));
                sockid = (int)(eleValue.scalarValue);
                // rtapi_print("socketopen: the first argument is %d\n", sockid);
            }

            ++nArgIndex;
            if (bracketClosed)
                break;

            arrEle = strtok(NULL, split);
        }

        CHKS(nArgIndex != 1, INTERP_ERR_SYNTAX_FAILED, _("socketclose function can only accept 1 arguments!"));
        CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));

        if (sockid > 0)
        {
            ::close(sockid);
            shutdown(sockid, SHUT_RDWR);
            sock_list.erase(remove(sock_list.begin(), sock_list.end(), sockid), sock_list.end());
            if (exists)
            {
                sock_id_param_val.valueType = SCALAR_VALUE;
                sock_id_param_val.scalarValue = -1;
                store_named_param(&_setup, sock_id_param_name, sock_id_param_val);
            }
        }
    }

    *counter = strlen(line);
    return INTERP_OK;
}

int Interp::read_socketreadreal(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    if (!MDImode)
    {
        SYNCH_PARSE_EXEC;
    }
    if (value_ptr)
    {
        value_ptr->valueType = ARRAY_VALUE;
        value_ptr->arrValue.clear();
    }

    int ret = INTERP_OK;
    if (!parsing_flag && !beforeProgStartLine)
    {
        int sockid = -1;
        double timeout = 2.0;
        int realNum = 0;

        while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
        CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
        *counter = *counter + 1;

        char argsText[LINELEN];
        strcpy(argsText, line + *counter);

        int nArgIndex = 0;
        bool bracketClosed = false;
        const char* split = ",";
        char* arrEle = strtok(argsText, split);
        while (arrEle != NULL)
        {
            char* pchar = strstr(arrEle, ")");
            if (pchar != NULL)
            {  // contain the ')'
                bracketClosed = true;
                *pchar = '\0';
            }

            jaka_value eleValue;
            int subCounter = 0;
            while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
                ++subCounter;

            ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
            CHK_INTERP_READ(ret, line, counter);
            if (nArgIndex == 0)
            {
                CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of socketreadreal can be only an integer value!"));
                sockid = round_to_int(eleValue.scalarValue);
                // rtapi_print("socketreadreal: the first argument is %d\n", sockid);
            }
            else if (nArgIndex == 1)
            {
                CHKS(eleValue.valueType != SCALAR_VALUE || (round_to_int(eleValue.scalarValue) <= 0),
                     INTERP_ERR_SYNTAX_FAILED,
                     _("The second argument of socketreadreal can be only a positive integer value!"));
                realNum = round_to_int(eleValue.scalarValue);
                if (value_ptr)
                    value_ptr->arrValue.resize(realNum, FLT_MAX);
                // rtapi_print("socketreadreal: the second argument is %d\n", realNum);
            }
            else if (nArgIndex == 2)
            {
                CHKS(eleValue.valueType != SCALAR_VALUE || (round_to_int(eleValue.scalarValue) < 0),
                     INTERP_ERR_SYNTAX_FAILED,
                     _("The third argument of socketreadreal can be only a non-negative real value!"));
                timeout = fabs(eleValue.scalarValue) < 10e-6 ? 65536 : eleValue.scalarValue;
            }

            ++nArgIndex;
            if (bracketClosed)
                break;
            arrEle = strtok(NULL, split);
        }

        CHKS(nArgIndex != 2 && nArgIndex != 3, INTERP_ERR_SYNTAX_FAILED, _("socketreadreal function can only accept 2 or 3 arguments!"));
        CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));

        int sentsize, recvsize;
        if (sockid < 0)
        {
            *counter = strlen(line);
            return INTERP_OK;
        }

        if (start_time == 0)
        {
            std::string cmdtosend = "get #real#" + std::to_string(realNum) + "#";
            sentsize = send(sockid, cmdtosend.c_str(), cmdtosend.size(), 0);
            if (sentsize < 0)
            {
                *counter = strlen(line);
                return INTERP_OK;
            }
            start_time = etime();
        }

        memset(cam_recv_buf, 0, sizeof(cam_recv_buf));
        recvsize = recv(sockid, cam_recv_buf, sizeof(cam_recv_buf), 0);
        if (recvsize > 0)
        {
            // rtapi_print("succeed to receive data: %s, size %d\n", cam_recv_buf, recvsize);
            std::string dataReceived(cam_recv_buf);
            std::string valueString("");
            int bracketFlag = 0;
            for (int i = 0; i < (int)dataReceived.size(); i++)
            {
                if (dataReceived[i] == '{' || dataReceived[i] == '[')
                {
                    bracketFlag = 1;  // reading value
                    continue;
                }
                else if ((dataReceived[i] == '}' || dataReceived[i] == ']') && bracketFlag == 1)
                {
                    bracketFlag = 2;  // complete
                    break;
                }
                if (bracketFlag == 1)
                {
                    valueString.push_back(dataReceived[i]);
                }
            }

            if (bracketFlag != 2)
            {
                start_time = 0;
                *counter = strlen(line);

                if (value_ptr)
                {  // abnomal receive
                    value_ptr->valueType = SCALAR_VALUE;
                    value_ptr->scalarValue = -1;
                    value_ptr->arrValue.clear();
                }
                rtapi_print("socketreadreal() recv ERROR. Expect recv data with format '{var}' or '[var]'>'\n");
                ERS(INTERP_WARNING_SOCKET_RECV_INVALID_DATA, "socketreadreal()  ERROR FORMAT. Expect recv data with format '{var}' or '[var]'>'");

                return INTERP_OK;
            }

            try
            {
                if (value_ptr)
                {
                    std::vector<std::string> v;
                    splitString(valueString, v, ",");
                    for (size_t i = 0; i < v.size() && i < (unsigned int)realNum; ++i) { value_ptr->arrValue[i] = std::stod(v[i]); }
                    if (v.size() > (unsigned int)realNum)
                    {
                        ERS(INTERP_WARNING_SOCKET_UNMATCHED_ARRAY_SIZE,
                            "socketreadreal()  Mismatched Array Size. Expect data:  'Array[%d]',Received Array[%d] '",
                            realNum,
                            (int)v.size());
                    }
                    // if (v.size() > (unsigned int)realNum)
                    // {
                    //     zucLogWarning(INTERP_WARNING_SOCKET_UNMATCHED_ARRAY_SIZE,
                    //                   "socketreadreal()  Mismatched Array Size. Expect data:  'Array[%d]',Received Array[%d] '",
                    //                   realNum,
                    //                   (int)v.size());
                    // }
                }
            }
            catch (...)
            {
                // rtapi_print("bad data format received: %s\n", cam_recv_buf);
                ERS(INTERP_WARNING_SOCKET_UNMATCHED_DATA_TYPE,
                    "socketreadreal()  Mismatched Data Type. Expect data:  '{doueble,double}' or '[double,doueble]'>'");
                start_time = 0;
                *counter = strlen(line);
                return INTERP_OK;
            }
        }
        else if (recvsize == 0)
        {  // socket closed by server side
            *counter = strlen(line);
            value_ptr->arrValue.clear();
            ERS(INTERP_ERR_SOCKET_CONNECT, "socket closed by server side");
        }
        else if (recvsize < 0 && (errno != EINTR && errno != EWOULDBLOCK && errno != EAGAIN))
        {
            value_ptr->arrValue.clear();
            rtapi_print("read_socketreadreal errno = %d\n", errno);
        }
        else
        {  // normal receive
            if (etime() - start_time < timeout)
            {
                esleep(0.1);
                WAIT_PARSE_EXEC;
            }
            else
            {  // receive timeout
                start_time = 0;
                *counter = strlen(line);
                value_ptr->arrValue.clear();
                rtapi_print("socket_readreal() timeout. \n");
                return INTERP_OK;
            }
        }
    }

    start_time = 0;
    *counter = strlen(line);

    EXECUTING_BLOCK(_setup).m_modes[11] = SOCKETREADREAL;
    return INTERP_OK;
}

int Interp::read_socketreadstring(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    if (!MDImode)
    {
        SYNCH_PARSE_EXEC;
    }
    if (value_ptr)
    {
        value_ptr->valueType = STRING_VALUE;
        value_ptr->strValue = std::string("");
    }
    int ret = INTERP_OK;
    if (!parsing_flag && !beforeProgStartLine)
    {
        int sockid = -1;
        std::string strPrefix("");
        std::string strSuffix("");
        double timeout = 2.0;

        while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
        CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
        *counter = *counter + 1;

        char argsText[LINELEN];
        strcpy(argsText, line + *counter);

        int nArgIndex = 0;
        bool bracketClosed = false;
        const char* split = ",";
        char* arrEle = strtok(argsText, split);
        while (arrEle != NULL)
        {
            char* pchar = strstr(arrEle, ")");
            if (pchar != NULL)
            {  // contain the ')'
                bracketClosed = true;
                *pchar = '\0';
            }

            jaka_value eleValue;
            int subCounter = 0;
            while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
                ++subCounter;

            ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
            CHK_INTERP_READ(ret, line, counter);
            if (nArgIndex == 0)
            {
                CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of socketreadstring can be only an integer value!"));
                sockid = round_to_int(eleValue.scalarValue);
                // rtapi_print("socketreadstring: the first argument is %d\n", sockid);
            }
            else if (nArgIndex == 1)
            {
                CHKS(eleValue.valueType != STRING_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The second argument of socketreadstring can be only a string value!"));
                strPrefix = eleValue.strValue;
                // rtapi_print("socketreadstring: the second argument is %s\n", strPrefix.c_str());
            }
            else if (nArgIndex == 2)
            {
                CHKS(eleValue.valueType != STRING_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The third argument of socketreadstring can be only a string value!"));
                strSuffix = eleValue.strValue;
                // rtapi_print("socketreadstring: the third argument is %s\n", strSuffix.c_str());
            }
            else if (nArgIndex == 3)
            {
                CHKS(eleValue.valueType != SCALAR_VALUE || (round_to_int(eleValue.scalarValue) < 0),
                     INTERP_ERR_SYNTAX_FAILED,
                     _("The forth argument of socketreadstring can be only a non-negative real value!"));
                timeout = fabs(eleValue.scalarValue) < 10e-6 ? 65536 : eleValue.scalarValue;
            }

            ++nArgIndex;
            if (bracketClosed)
                break;
            arrEle = strtok(NULL, split);
        }

        CHKS(nArgIndex != 3 && nArgIndex != 4, INTERP_ERR_SYNTAX_FAILED, _("socketreadstring function can only accept 3 or 4 arguments!"));
        CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));

        int sentsize, recvsize;
        if (sockid < 0)
        {
            if (value_ptr)
            {
                value_ptr->valueType = SCALAR_VALUE;
                value_ptr->scalarValue = -1;
            }
            *counter = strlen(line);
            return INTERP_OK;
        }

        if (start_time == 0)
        {
            std::string cmdtosend = "get #string#" + strPrefix + "#" + strSuffix + "#";
            sentsize = send(sockid, cmdtosend.c_str(), cmdtosend.size(), 0);
            if (sentsize < 0)
            {
                if (value_ptr)
                {
                    value_ptr->valueType = SCALAR_VALUE;
                    value_ptr->scalarValue = -1;
                }
                *counter = strlen(line);
                return INTERP_OK;
            }
            start_time = etime();
        }

        memset(cam_recv_buf, 0, sizeof(cam_recv_buf));
        recvsize = recv(sockid, cam_recv_buf, sizeof(cam_recv_buf), 0);
        if (recvsize > 0)
        {
            std::string s(cam_recv_buf);
            std::regex reg(strPrefix + "(.*)" + strSuffix);
            std::smatch sm;
            if (std::regex_match(s, sm, reg) && value_ptr)
            {
                value_ptr->strValue = sm[1].str();
                // rtapi_print("variable get successfully: %s\n\n", value_ptr->strValue.c_str());
            }
            else
            {
                if (value_ptr)
                {  // abnomal receive
                    value_ptr->valueType = SCALAR_VALUE;
                    value_ptr->scalarValue = -1;
                }
                rtapi_print("socketreadstring() recv ERROR. Expect recv data with format '%s STRING %s'\n", strPrefix.c_str(), strSuffix.c_str());
                ERS(INTERP_WARNING_SOCKET_RECV_INVALID_DATA, "socketreadstring() recv ERROR");
            }
        }
        else if (recvsize == 0)
        {  // socket closed by server side
            *counter = strlen(line);
            ERS(INTERP_ERR_SOCKET_CONNECT, "socket closed by server side");
        }
        else if (recvsize < 0 && errno != EAGAIN && value_ptr)
        {
            // suppose it to be connection error
            value_ptr->valueType = SCALAR_VALUE;
            value_ptr->scalarValue = -1;
        }
        else if (recvsize < 0 && (errno != EINTR && errno != EWOULDBLOCK && errno != EAGAIN))
        {
            rtapi_print("read_socketreadstring errno = %d\n", errno);
        }
        else
        {  // normal receive
            if (etime() - start_time < timeout)
            {
                esleep(0.1);
                WAIT_PARSE_EXEC;
            }
            else
            {  // receive timeout
                start_time = 0;
                *counter = strlen(line);
                rtapi_print("socket_readstring() timeout. \n");
                return INTERP_OK;
            }
        }
    }

    start_time = 0;
    *counter = strlen(line);

    EXECUTING_BLOCK(_setup).m_modes[11] = SOCKETREADSTRING;
    return INTERP_OK;
}

int Interp::read_socketsend(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    if (!MDImode)
    {
        SYNCH_PARSE_EXEC;
    }

    int ret = INTERP_OK;
    int sentsize = 0;
    if (value_ptr)
    {
        value_ptr->valueType = SCALAR_VALUE;
        value_ptr->scalarValue = -1;
    }

    if (!parsing_flag && !beforeProgStartLine)
    {
        int sockid = -1;
        std::string strtosend("");

        while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
        CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
        *counter = *counter + 1;

        char argsText[LINELEN];
        strcpy(argsText, line + *counter);

        int nArgIndex = 0;
        bool bracketClosed = false;
        const char* split = ",";
        char* arrEle = strtok(argsText, split);
        while (arrEle != NULL)
        {
            char* pchar = strstr(arrEle, ")");
            if (pchar != NULL)
            {  // contain the ')'
                bracketClosed = true;
                *pchar = '\0';
            }

            jaka_value eleValue;
            int subCounter = 0;
            while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
                ++subCounter;

            ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
            CHK_INTERP_READ(ret, line, counter);
            if (nArgIndex == 0)
            {
                CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of socketsend can be only an integer value!"));
                sockid = round_to_int(eleValue.scalarValue);
                rtapi_print("socketsend: the first argument is %d\n", sockid);
            }
            else if (nArgIndex == 1)
            {
                strtosend = "";
                if (eleValue.valueType == SCALAR_VALUE)
                {
                    strtosend = doubleToString(eleValue.scalarValue);
                }
                else if (eleValue.valueType == ARRAY_VALUE)
                {
                    for (int k = 0; k < (int)eleValue.arrValue.size(); ++k)
                    {
                        if (k == 0)
                        {
                            strtosend = strtosend + doubleToString(eleValue.arrValue[k]);
                        }
                        else
                        {
                            strtosend = strtosend + ", " + doubleToString(eleValue.arrValue[k]);
                        }
                    }
                }
                else
                {
                    strtosend = eleValue.strValue;
                }
                rtapi_print("socketsend: the second argument is %s\n", strtosend.c_str());
            }

            ++nArgIndex;
            if (bracketClosed)
                break;
            arrEle = strtok(NULL, split);
        }

        CHKS(nArgIndex != 2, INTERP_ERR_SYNTAX_FAILED, _("socketsend function can only accept 2 arguments!"));
        CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));

        if (sockid < 0)
        {
            if (value_ptr)
                value_ptr->scalarValue = -1;  // return value
            *counter = strlen(line);
            return INTERP_OK;
        }

        sentsize = send(sockid, strtosend.c_str(), strtosend.size(), 0);
        if (sentsize < 0)
        {
            if (value_ptr)
                value_ptr->scalarValue = -1;  // return value
            *counter = strlen(line);
            // rtapi_print("socket send failed, errno is %d\n", errno);
            return INTERP_OK;
        }
    }

    *counter = strlen(line);
    if (value_ptr)
        value_ptr->scalarValue = sentsize;  // return value

    EXECUTING_BLOCK(_setup).m_modes[11] = SOCKETSEND;
    return INTERP_OK;
}

int Interp::read_socketsendbyte(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    if (!MDImode)
    {
        SYNCH_PARSE_EXEC;
    }

    int ret = INTERP_OK;
    int sentsize = 0;
    if (value_ptr)
    {
        value_ptr->valueType = SCALAR_VALUE;
        value_ptr->scalarValue = -1;
    }

    if (!parsing_flag && !beforeProgStartLine)
    {
        int sockid = -1;
        unsigned char buf[2];
        while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
        CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
        *counter = *counter + 1;

        char argsText[LINELEN];
        strcpy(argsText, line + *counter);

        int nArgIndex = 0;
        bool bracketClosed = false;
        const char* split = ",";
        char* arrEle = strtok(argsText, split);
        while (arrEle != NULL)
        {
            char* pchar = strstr(arrEle, ")");
            if (pchar != NULL)
            {  // contain the ')'
                bracketClosed = true;
                *pchar = '\0';
            }

            jaka_value eleValue;
            int subCounter = 0;
            while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
                ++subCounter;

            ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
            CHK_INTERP_READ(ret, line, counter);
            if (nArgIndex == 0)
            {
                CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of socketsendbyte can be only an integer value!"));
                sockid = round_to_int(eleValue.scalarValue);
            }
            else if (nArgIndex == 1)
            {
                CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The second argument of socketsendbyte can be only an integer value!"));
                buf[0] = round_to_int(eleValue.scalarValue);
            }

            ++nArgIndex;
            if (bracketClosed)
                break;
            arrEle = strtok(NULL, split);
        }

        CHKS(nArgIndex != 2, INTERP_ERR_SYNTAX_FAILED, _("socketsendbyte function can only accept 2 arguments!"));
        CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));

        if (sockid < 0)
        {
            if (value_ptr)
                value_ptr->scalarValue = -1;  // return value
            *counter = strlen(line);
            return INTERP_OK;
        }

        sentsize = send(sockid, buf, 1, 0);
        if (sentsize < 0)
        {
            if (value_ptr)
                value_ptr->scalarValue = -1;  // return value
            *counter = strlen(line);
            return INTERP_OK;
        }
    }

    *counter = strlen(line);
    if (value_ptr)
        value_ptr->scalarValue = sentsize;  // return value

    EXECUTING_BLOCK(_setup).m_modes[11] = SOCKETSENDBYTE;
    return INTERP_OK;
}

int Interp::read_sprintf(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    SYNCH_PARSE_EXEC;
    int ret = INTERP_OK;
    if (value_ptr)
    {
        value_ptr->valueType = SCALAR_VALUE;
        value_ptr->scalarValue = 0;
    }
    if (!parsing_flag)
    {
        std::vector<int> fmtArgTypes;      // 0: int; 1: double; 2: string
        std::vector<std::string> fmtArgs;  // string of format arguments
        std::string strFormat("");
        while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
        CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
        *counter = *counter + 1;

        char argsText[LINELEN];
        strcpy(argsText, line + *counter);

        int nArgIndex = 0;
        bool bracketClosed = false;
        const char* split = ",";
        char arrName[256];
        char* arrEle = strtok(argsText, split);
        while (arrEle != NULL)
        {
            char* pchar = strstr(arrEle, ")");
            if (pchar != NULL)
            {  // contain the ')'
                bracketClosed = true;
                *pchar = '\0';
            }

            jaka_value eleValue;
            int subCounter = 0;
            while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
                ++subCounter;
            ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
            CHK_INTERP_READ(ret, line, counter);
            if (nArgIndex == 0)
            {
                subCounter = 0;
                read_name(arrEle, &subCounter, arrName);
                rtapi_print("sprintf: read name %s\n", arrName);
            }
            else if (nArgIndex == 1)
            {
                CHKS(eleValue.valueType != STRING_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of sprintf can be only a string value!"));
                /*parse the format in the string, get the arguments number*/
                strFormat = eleValue.strValue;
                for (size_t i = 0; i < strFormat.size();)
                {
                    if (strFormat.compare(i, 2, "%d") == 0)
                    {
                        fmtArgTypes.push_back(0);
                        i += 2;
                    }
                    else if (strFormat.compare(i, 2, "%f") == 0)
                    {
                        fmtArgTypes.push_back(1);
                        i += 2;
                    }
                    else if (strFormat.compare(i, 2, "%s") == 0)
                    {
                        fmtArgTypes.push_back(2);
                        i += 2;
                    }
                    else
                    {
                        ++i;
                    }
                }
            }
            else
            {
                if (nArgIndex - 2 < (int)fmtArgTypes.size())
                {
                    if (fmtArgTypes[nArgIndex - 2] == 0 && eleValue.valueType == SCALAR_VALUE)
                    {  // int
                        fmtArgs.push_back(std::to_string(int(eleValue.scalarValue)));
                    }
                    else if (fmtArgTypes[nArgIndex - 2] == 1 && eleValue.valueType == SCALAR_VALUE)
                    {  // float
                        std::string res = doubleToString(eleValue.scalarValue);
                        fmtArgs.push_back(res);
                    }
                    else if (fmtArgTypes[nArgIndex - 2] == 2 && eleValue.valueType == STRING_VALUE)
                    {  // string
                        fmtArgs.push_back(eleValue.strValue);
                    }
                    else
                    {
                        if (value_ptr)
                            value_ptr->scalarValue = -1;  // umatched data type
                        *counter = strlen(line);
                        return INTERP_OK;
                    }
                }
            }

            ++nArgIndex;
            if (bracketClosed)
                break;
            arrEle = strtok(NULL, split);
        }

        CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
        for (size_t i = 0; i < fmtArgTypes.size() && i < fmtArgs.size(); ++i)
        {
            if (fmtArgTypes[i] == 0)
            {
                strFormat = strFormat.replace(strFormat.find("%d"), 2, fmtArgs[i]);
            }
            else if (fmtArgTypes[i] == 1)
            {
                strFormat = strFormat.replace(strFormat.find("%f"), 2, fmtArgs[i]);
            }
            else
            {
                strFormat = strFormat.replace(strFormat.find("%s"), 2, fmtArgs[i]);
            }
        }
        jaka_value formatedStr;
        formatedStr.valueType = STRING_VALUE;
        formatedStr.strValue = strFormat;
        const char* dup = strstore(arrName);  // no more need to free this
        if (dup == 0)
        {
            ERS(INTERP_ERR_OUT_OF_MEMORY, NCE_OUT_OF_MEMORY);
        }
        CHP(store_named_param(&_setup, dup, formatedStr));
        if (value_ptr)
            value_ptr->scalarValue = strFormat.size();
        rtapi_print("After replacing string: %s\n", strFormat.c_str());
    }

    *counter = strlen(line);

    EXECUTING_BLOCK(_setup).m_modes[11] = SPRINTF;
    return INTERP_OK;
}

#define MAXLN 512
#define ISSPACE " \t\n\r\f\v"
int Interp::do_sscanf(const char* buf, const char* s, double* parameters, std::vector<std::string> ap)
{
    int count, width, base = 0;
    // const char     *tc;
    char /* *t,  */ tmp[MAXLN];
    char dbVal[MAXLN];
    jaka_value temp_val;
    std::vector<jaka_value> mres;

    count = width = 0;
    while (*s && *buf)
    {
        while (isspace(*s)) s++;
        if (*s == '%')
        {
            s++;
            for (; *s; s++)
            {
                if (strchr("douxsf%", *s))
                    break;
                // else if (*s >= '1' && *s <= '9') {	// to support %5d
                // 	for (tc = s; isdigit(*s); s++);
                // 	strncpy(tmp, tc, s - tc);
                // 	tmp[s - tc] = '\0';
                // 	width = strtol(tmp, NULL, 10);
                // 	s--;
                // }
            }
            if (*s == 's')
            {
                while (isspace(*buf)) buf++;
                if (!width)
                {
                    if (*(s + 1) == 0)
                    {
                        width = strlen(buf);
                        strncpy(dbVal, buf, 512);
                    }
                    else if (isspace(*(s + 1)))
                    {
                        width = strcspn(buf, ISSPACE);
                        strncpy(dbVal, buf, 512);
                    }
                    else
                    {
                        if (*(s + 1) == '%' && *(s + 2) && (*(s + 2) == 's' || *(s + 2) == 'f' || *(s + 2) == 'd'))
                        {
                            width = strcspn(buf, ISSPACE);
                            strncpy(dbVal, buf, 512);
                        }
                        else
                        {
                            ++s;
                            std::string tmpStr(1, *s);  // char to str
                            width = strcspn(buf, tmpStr.c_str());
                            strncpy(dbVal, buf, 512);
                            ++buf;
                        }
                    }
                }

                buf += width;
                dbVal[width] = '\0';
                temp_val.valueType = STRING_VALUE;
                temp_val.strValue = std::string(dbVal);
                mres.push_back(temp_val);
                // printf("matched string: %s\n", dbVal);
            }
            else if (strchr("dobxu", *s))
            {
                while (isspace(*buf)) buf++;
                if (*s == 'd' || *s == 'u')
                    base = 10;
                else if (*s == 'x')
                    base = 16;
                else if (*s == 'o')
                    base = 8;
                else if (*s == 'b')
                    base = 2;
                if (!width)
                {
                    if (isspace(*(s + 1)) || *(s + 1) == 0)
                        width = strcspn(buf, ISSPACE);
                    else
                        width = strchr(buf, *(s + 1)) - buf;
                }
                strncpy(tmp, buf, width);
                tmp[width] = '\0';
                buf += width;
                temp_val.valueType = SCALAR_VALUE;
                temp_val.scalarValue = strtol(tmp, NULL, base);
                mres.push_back(temp_val);
                // printf("matched number: %f\n", temp_val.scalarValue);
            }
            else if (strchr("f", *s))
            {
                while (isspace(*buf)) buf++;
                width = strspn(buf, "+-");
                width = strspn(buf + width, "0123456789.") + width;
                std::string st(buf, buf + width);
                std::stringstream s(st);
                temp_val.valueType = SCALAR_VALUE;
                s >> temp_val.scalarValue;
                if (s.get() != std::char_traits<char>::eof())
                {
                    // printf("value bad number format\n");
                    break;
                }
                buf += width;
                mres.push_back(temp_val);
                // printf("matched number: %f\n", temp_val.scalarValue);
            }
            else if (strchr("%", *s))
            {
                s--;
                buf++;
            }
            count++;
            width = 0;
            s++;
        }
        else
        {
            while (isspace(*buf)) buf++;
            if (*s != *buf)
                break;
            else
                s++, buf++;
        }
    }

    for (size_t i = 0; i < (unsigned long)count && i < ap.size(); i++)
    {
        const char* dup = strstore(ap[i].c_str());
        if (store_named_param(&_setup, dup, mres[i]) != INTERP_OK)
        {
            return -1;
        }
    }
    return (count);
}

int Interp::read_sscanf(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    SYNCH_PARSE_EXEC;
    int ret = INTERP_OK;
    if (value_ptr)
    {
        value_ptr->valueType = SCALAR_VALUE;
        value_ptr->scalarValue = 0;
    }
    if (!parsing_flag)
    {
        std::vector<int> fmtArgTypes;       // 0: int; 1: double; 2: string
        std::vector<std::string> argNames;  // string of format arguments
        std::string strFormat("");
        std::string strFormatInRegex("");
        std::string strBuffer("");
        char varname[128] = "";
        while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
        CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
        *counter = *counter + 1;

        char argsText[LINELEN];
        strcpy(argsText, line + *counter);

        int nArgIndex = 0;
        bool bracketClosed = false;
        const char* split = ",";
        char* arrEle = strtok(argsText, split);
        while (arrEle != NULL)
        {
            char* pchar = strstr(arrEle, ")");
            if (pchar != NULL)
            {  // contain the ')'
                bracketClosed = true;
                *pchar = '\0';
            }

            jaka_value eleValue;
            int subCounter = 0;
            while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
                ++subCounter;
            ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
            CHK_INTERP_READ(ret, line, counter);
            if (nArgIndex == 0)
            {
                CHKS(eleValue.valueType != STRING_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of sscanf can be only a string value!"));
                strBuffer = eleValue.strValue;
            }
            else if (nArgIndex == 1)
            {
                CHKS(eleValue.valueType != STRING_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The second argument of sscanf can be only a string value!"));
                strFormat = eleValue.strValue;
            }
            else
            {
                subCounter = 0;
                read_name(arrEle, &subCounter, varname);
                argNames.push_back(std::string(varname));
            }

            ++nArgIndex;
            if (bracketClosed)
                break;
            arrEle = strtok(NULL, split);
        }
        CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
        int matchres = do_sscanf(strBuffer.c_str(), strFormat.c_str(), parameters, argNames);
        if (matchres == -1)
        {
            ERS(INTERP_ERR_INTERNAL, "sscanf error");
        }
        if (value_ptr)
            value_ptr->scalarValue = matchres;
    }

    *counter = strlen(line);

    EXECUTING_BLOCK(_setup).m_modes[11] = SSCANF;
    return INTERP_OK;
}

int Interp::read_getstringfromarray(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    SYNCH_PARSE_EXEC;
    int ret = INTERP_OK;
    if (value_ptr)
    {
        value_ptr->valueType = SCALAR_VALUE;  // return string size
        value_ptr->scalarValue = 0;
    }

    if (!parsing_flag)
    {
        jaka_value arrValue;
        std::string separatorToken("");
        char varname[128] = "";
        while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
        CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
        *counter = *counter + 1;

        char argsText[LINELEN];
        strcpy(argsText, line + *counter);

        int nArgIndex = 0;
        bool bracketClosed = false;
        const char* split = ",";
        char* arrEle = strtok(argsText, split);
        while (arrEle != NULL)
        {
            char* pchar = strstr(arrEle, ")");
            if (pchar != NULL)
            {  // contain the ')'
                bracketClosed = true;
                *pchar = '\0';
            }

            jaka_value eleValue;
            int subCounter = 0;
            while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
                ++subCounter;

            ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
            CHK_INTERP_READ(ret, line, counter);
            if (nArgIndex == 0)
            {
                CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of getstringfromarray can be only an array value!"));
                arrValue = eleValue;
            }
            else if (nArgIndex == 1)
            {
                CHKS(eleValue.valueType != STRING_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The second argument of getstringfromarray can be only a string value!"));
                separatorToken = eleValue.strValue;
            }
            else if (nArgIndex == 2)
            {
                CHKS(eleValue.valueType != STRING_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The third argument of getstringfromarray can be only a string value!"));
                subCounter = 0;
                read_name(arrEle, &subCounter, varname);
            }
            ++nArgIndex;
            if (bracketClosed)
                break;
            arrEle = strtok(NULL, split);
        }

        CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));

        jaka_value resVal;
        resVal.valueType = STRING_VALUE;
        for (size_t i = 0; i < arrValue.arrValue.size(); ++i)
        {
            resVal.strValue += (i == 0) ? "" : separatorToken;
            resVal.strValue += doubleToString(arrValue.arrValue[i]);
        }
        CHP(store_named_param(&_setup, varname, resVal));
        if (value_ptr)
            value_ptr->scalarValue = resVal.strValue.size();
    }

    *counter = strlen(line);
    return INTERP_OK;
}

int Interp::read_getarrayfromstring(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    SYNCH_PARSE_EXEC;
    int ret = INTERP_OK;
    if (value_ptr)
    {
        value_ptr->valueType = SCALAR_VALUE;
        value_ptr->scalarValue = 0;
    }

    if (!parsing_flag)
    {
        std::string arrValueInString;
        std::string separatorToken("");
        char varname[128] = "";
        while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
        CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
        *counter = *counter + 1;

        char argsText[LINELEN];
        strcpy(argsText, line + *counter);

        int nArgIndex = 0;
        bool bracketClosed = false;
        const char* split = ",";
        char* arrEle = strtok(argsText, split);
        while (arrEle != NULL)
        {
            char* pchar = strstr(arrEle, ")");
            if (pchar != NULL)
            {  // contain the ')'
                bracketClosed = true;
                *pchar = '\0';
            }

            jaka_value eleValue;
            int subCounter = 0;
            while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
                ++subCounter;

            ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
            CHK_INTERP_READ(ret, line, counter);
            if (nArgIndex == 0)
            {
                CHKS(eleValue.valueType != STRING_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of getarrayfromstring can be only a string value!"));
                arrValueInString = eleValue.strValue;
            }
            else if (nArgIndex == 1)
            {
                CHKS(eleValue.valueType != STRING_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The second argument of getarrayfromstring can be only a string value!"));
                separatorToken = eleValue.strValue;
            }
            else if (nArgIndex == 2)
            {
                CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The third argument of getarrayfromstring can be only an array value!"));
                subCounter = 0;
                read_name(arrEle, &subCounter, varname);
            }
            ++nArgIndex;
            if (bracketClosed)
                break;
            arrEle = strtok(NULL, split);
        }

        CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
        try
        {
            std::vector<std::string> v;
            splitString(arrValueInString, v, separatorToken);
            jaka_value resVal;
            resVal.valueType = ARRAY_VALUE;
            resVal.arrValue.resize(v.size());
            for (size_t i = 0; i < v.size(); ++i) { resVal.arrValue[i] = std::stod(v[i]); }
            CHP(store_named_param(&_setup, varname, resVal));
            if (value_ptr)
                value_ptr->scalarValue = v.size();
        }
        catch (std::exception& e)
        {
            rtapi_print("getarrayfromstring excetion: %s\n", e.what());
            if (value_ptr)
                value_ptr->scalarValue = -1;
            *counter = strlen(line);
            return INTERP_OK;
        }
    }

    *counter = strlen(line);
    return INTERP_OK;
}

int Interp::read_stringconcat(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    SYNCH_PARSE_EXEC;
    int ret = INTERP_OK;
    if (value_ptr == nullptr)
    {
        *counter = strlen(line);
        return INTERP_OK;
    }
    value_ptr->valueType = STRING_VALUE;
    value_ptr->strValue = std::string("");

    if (!parsing_flag)
    {
        std::string strRes("");

        while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
        CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
        *counter = *counter + 1;

        char argsText[LINELEN];
        strcpy(argsText, line + *counter);

        int nArgIndex = 0;
        bool bracketClosed = false;
        const char* split = ",";
        char* arrEle = strtok(argsText, split);
        while (arrEle != NULL)
        {
            char* pchar = strstr(arrEle, ")");
            if (pchar != NULL)
            {  // contain the ')'
                bracketClosed = true;
                *pchar = '\0';
            }

            jaka_value eleValue;
            int subCounter = 0;
            while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
                ++subCounter;

            ret = read_real_value(arrEle, &subCounter, &eleValue, parameters); /*  */
            CHK_INTERP_READ(ret, line, counter);
            if (nArgIndex == 0)
            {
                CHKS(eleValue.valueType != STRING_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of stringconcat can be only a string value!"));
                strRes = eleValue.strValue;
            }
            else if (nArgIndex == 1)
            {
                // CHKS(eleValue.valueType != STRING_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The second argument of stringconcat can be only a string value!"));
                // strRes += eleValue.strValue;
                switch (eleValue.valueType)
                {
                case STRING_VALUE:
                    strRes += eleValue.strValue;
                    break;
                case SCALAR_VALUE:
                    strRes += std::to_string(eleValue.scalarValue);
                    break;
                case ARRAY_VALUE: {
                    strRes += "[";
                    int arrSize = eleValue.arrValue.size();
                    for (int i = 0; i < arrSize; i++)
                    {
                        strRes += std::to_string(eleValue.arrValue.at(i));
                        if (i < arrSize - 1)
                        {
                            strRes += ", ";
                        }
                    }
                    strRes += "]";
                }
                break;
                default:
                    ERS(INTERP_ERR_SYNTAX_FAILED, _("The second argument of stringconcat can be only an integer(or  an array or a string ) value!"));
                    break;
                }
            }

            ++nArgIndex;
            if (bracketClosed)
                break;
            arrEle = strtok(NULL, split);
            // printf("%s\n", eleValue.strValue.c_str());
        }

        CHKS(nArgIndex != 2, INTERP_ERR_SYNTAX_FAILED, _("stringconcat can only accept 2 arguments!"));
        CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
        value_ptr->strValue = strRes;
        // rtapi_print("After concat string: %s\n", strRes.c_str());
    }

    *counter = strlen(line);

    EXECUTING_BLOCK(_setup).m_modes[11] = STRINGCONCAT;
    return INTERP_OK;
}

/* compare two strings */
int Interp::read_strcmp(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    SYNCH_PARSE_EXEC;
    int ret = INTERP_OK;
    if (value_ptr == nullptr)
    {
        *counter = strlen(line);
        return INTERP_OK;
    }
    value_ptr->valueType = SCALAR_VALUE;
    value_ptr->scalarValue = 0;
    std::string strsrc("");
    std::string strdst("");
    if (!parsing_flag)
    {
        while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
        CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
        *counter = *counter + 1;

        char argsText[LINELEN];
        strcpy(argsText, line + *counter);

        int nArgIndex = 0;
        bool bracketClosed = false;
        const char* split = ",";
        char* arrEle = strtok(argsText, split);
        while (arrEle != NULL)
        {
            char* pchar = strstr(arrEle, ")");
            if (pchar != NULL)
            {  // contain the ')'
                bracketClosed = true;
                *pchar = '\0';
            }

            jaka_value eleValue;
            int subCounter = 0;
            while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
                ++subCounter;

            ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
            CHK_INTERP_READ(ret, line, counter);
            if (nArgIndex == 0)
            {
                CHKS(eleValue.valueType != STRING_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of strcmp can be only a string value!"));
                strsrc = eleValue.strValue;
            }
            else if (nArgIndex == 1)
            {
                CHKS(eleValue.valueType != STRING_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The second argument of strcmp can be only a string value!"));
                strdst = eleValue.strValue;
            }

            ++nArgIndex;
            if (bracketClosed)
                break;
            arrEle = strtok(NULL, split);
        }

        CHKS(nArgIndex != 2, INTERP_ERR_SYNTAX_FAILED, _("strcmp can only accept 2 arguments!"));
        CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
        ret = strcmp(strsrc.c_str(), strdst.c_str());
        value_ptr->scalarValue = ret;
    }

    *counter = strlen(line);
    // printf("the strings that compared are %s and %s\n, the result is %f\n", strsrc.c_str(), strdst.c_str(), value_ptr->scalarValue);

    EXECUTING_BLOCK(_setup).m_modes[11] = STRINGCMP;
    return INTERP_OK;
}

int Interp::read_settool(char* line, int* counter, block_pointer block, double* parameters)
{
    if (_setup.defining_sub)
        return INTERP_OK;

    SYNCH_PARSE_EXEC;

    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);

    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        jaka_value eleValue;
        int subCounter = 0;

        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;

        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters); /* read_real_value ? */
        CHK_INTERP_READ(ret, line, counter);

        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of settool can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 6, INTERP_ERR_SYNTAX_FAILED, _("The position variable can only have 6 components!"));
            for (size_t i = 0; i < eleValue.arrValue.size(); ++i) block->tool_offset[i] = eleValue.arrValue[i];
        }

        ++nArgIndex;
        if (bracketClosed)
            break;

        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 1, INTERP_ERR_SYNTAX_FAILED, _("settool function can only accept 1 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
    *counter = strlen(line);

    block->g_modes[GM_MODAL_0] = JK_SETTOOL;

    return INTERP_OK;
}

int Interp::read_settoolid(char* line, int* counter, block_pointer block, double* parameters)
{
    if (_setup.defining_sub)
        return INTERP_OK;

    SYNCH_PARSE_EXEC;

    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);

    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        jaka_value eleValue;
        int subCounter = 0;

        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;

        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);

        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The  argument of settoolid can be a int value!"));
            block->tool_id = eleValue.scalarValue;  // zero --> false, non-zero --> true
        }

        ++nArgIndex;
        if (bracketClosed)
            break;

        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 1, INTERP_ERR_SYNTAX_FAILED, _("settoolid function can only accept 1 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
    *counter = strlen(line);

    block->g_modes[GM_MODAL_0] = JK_SETTOOLID;

    return INTERP_OK;
}

int Interp::read_setuserframe(char* line, int* counter, block_pointer block, double* parameters)
{
    if (_setup.defining_sub)
        return INTERP_OK;

    SYNCH_PARSE_EXEC;

    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);

    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        jaka_value eleValue;
        int subCounter = 0;

        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;

        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);

        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of settool can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 6, INTERP_ERR_SYNTAX_FAILED, _("The position variable can only have 6 components!"));
            for (size_t i = 0; i < eleValue.arrValue.size(); ++i) block->user_frame[i] = eleValue.arrValue[i];
        }

        ++nArgIndex;
        if (bracketClosed)
            break;

        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 1, INTERP_ERR_SYNTAX_FAILED, _("setuserframe function can only accept 1 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
    *counter = strlen(line);

    block->g_modes[GM_MODAL_0] = JK_SETUSERFRAME;

    return INTERP_OK;
}

int Interp::read_setuserid(char* line, int* counter, block_pointer block, double* parameters)
{
    if (_setup.defining_sub)
        return INTERP_OK;

    SYNCH_PARSE_EXEC;

    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);

    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        jaka_value eleValue;
        int subCounter = 0;

        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;

        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);

        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The  argument of setuserid can be a int value!"));
            block->user_id = eleValue.scalarValue;  // zero --> false, non-zero --> true
        }

        ++nArgIndex;
        if (bracketClosed)
            break;

        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 1, INTERP_ERR_SYNTAX_FAILED, _("setuserid function can only accept 1 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
    *counter = strlen(line);

    block->g_modes[GM_MODAL_0] = JK_SETUSERFRAMEID;

    return INTERP_OK;
}

int Interp::read_setpayload(char* line, int* counter, block_pointer block, double* parameters)
{
    if (_setup.defining_sub)
        return INTERP_OK;  //INTERP_OK  = 0
    SYNCH_PARSE_EXEC;
    rtapi_print("line is : %s\n", line);
    rtapi_print("parameters is : %p\n", parameters);
    rtapi_print("counter is : %d\n", *counter);
    // skip the space character, and check the '(' token
    while (*counter < (int)strlen(line) && line[*counter] == ' ')
    {
        *counter = *counter + 1;
        rtapi_print("counter is : %d\n", *counter);
    }
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;
    rtapi_print("counter is : %d\n", *counter);
    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char argsText[LINELEN];                         // LINELEN = 255
    strcpy(argsText, line + *counter);              //setpayload's length is 10 ,从7开始就是要设置的参数
    rtapi_print("argsText  is:   %s\n", argsText);  // argsText is :7,#<payload>)
    char* arrEle = strtok(argsText, split);
    rtapi_print("1 arrEle  is:   %s\n", arrEle);  //arrEle is : 7
    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        rtapi_print("pchar  is:   %s\n", pchar);
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        int subCounter = 0;
        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
        {
            ++subCounter;
        }

        jaka_value eleValue;
        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);

        if (nArgIndex == 0)
        {  //mass of payload   SCALAR_VALUE = 0
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The second argument of setpayload can be only a real value!"));
            block->payload.m = (fabs(eleValue.scalarValue));  // zero --> false, non-zero --> true
        }
        else if (nArgIndex == 1)
        {  // center of mass
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The second argument of setpayload can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 3, INTERP_ERR_SYNTAX_FAILED, _("The position variable can only have 3 components!"));
            for (size_t i = 0; i < eleValue.arrValue.size(); ++i)
            {
                block->payload.p[i] = eleValue.arrValue[i];
                rtapi_print("block->payload.p[%d]  is: %f\n", (int)i, eleValue.arrValue[i]);
            }
        }

        ++nArgIndex;
        if (bracketClosed)
            break;

        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 2, INTERP_ERR_SYNTAX_FAILED, _("setpayload function can only accept 2 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
    *counter = strlen(line);

    block->g_modes[GM_MODAL_0] = JK_SETPAYLOAD;
    return INTERP_OK;
}

int Interp::read_setadmitctrlconfig(char* line, int* counter, block_pointer block, double* parameters)
{
    if (_setup.defining_sub)
        return INTERP_OK;

    // SYNCH_PARSE_EXEC;

    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);
    //rtapi_print("admitctrlconfig: argsText is %s\n", argsText);

    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        jaka_value eleValue;
        int subCounter = 0;

        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;
        //rtapi_print("AdmitCtrlConfig: the text to read value: %s\n", arrEle);

        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);

        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of admitctrlconfig can an interger!"));
            block->utilNum = round_to_int(eleValue.scalarValue);
            //rtapi_print("admitctrlconfig: the first argument retrieved!\n");
        }
        else if (nArgIndex == 1)
        {
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The second argument of admitctrlconfig can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 5, INTERP_ERR_SYNTAX_FAILED, _("The admitctrlconfig variable can only have 5 components!"));
            block->ftConfig.enableFt = round_to_int(eleValue.arrValue[0]);
            block->ftConfig.ftUser = eleValue.arrValue[1];
            block->ftConfig.ftReboundFK = eleValue.arrValue[2];
            block->ftConfig.ftConstant = eleValue.arrValue[3];
            block->ftConfig.enableNTrack = round_to_int(eleValue.arrValue[4]);
            //rtapi_print("admitctrlconfig: the second argument retrieved!\n");
        }
        else if (nArgIndex == 2)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The third argument of admitctrlconfig can an interger!"));
            block->out_imme_flag = round_to_int(eleValue.scalarValue);
            //rtapi_print("admitctrlconfig: the first argument retrieved!\n");
        }
        ++nArgIndex;
        if (bracketClosed)
            break;

        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 3, INTERP_ERR_SYNTAX_FAILED, _("admitctrlconfig function can only accept 3 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
    *counter = strlen(line);
    block->g_modes[GM_MODAL_0] = JK_SETADMITCTRLCONFIG;
    return INTERP_OK;
}

int Interp::read_setendforcecondition(char* line, int* counter, block_pointer block, double* parameters)
{
    if (_setup.defining_sub)
        return INTERP_OK;

    // SYNCH_PARSE_EXEC;
    if (endForceCondFlag == 2)
    {
        rtapi_print("endForceCondFlag------: %d, %s\n", endForceCondFlag, line);
        set_endforcecond_flag(0);
        SYNCH_PARSE_EXEC;
    }

    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);
    //rtapi_print("EndForceCond: argsText is %s\n", argsText);

    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        jaka_value eleValue;
        int subCounter = 0;

        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;
        //rtapi_print("EndForceCond: the text to read value: %s\n", arrEle);

        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);

        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of EndForceCond can an interger!"));
            block->utilNum = round_to_int(eleValue.scalarValue);
            //rtapi_print("EndForceCond: the first argument retrieved!\n");
        }
        else if (nArgIndex == 1)
        {
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The second argument of EndForceCond can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 5, INTERP_ERR_SYNTAX_FAILED, _("The EndForceCond variable can only have 5 components!"));
            block->endForceCond.enableEndCond = round_to_int(eleValue.arrValue[0]);
            block->endForceCond.lowFlag = round_to_int(eleValue.arrValue[1]);
            block->endForceCond.lowLimitForce = eleValue.arrValue[2];
            block->endForceCond.upFlag = round_to_int(eleValue.arrValue[3]);
            block->endForceCond.upLimitForce = eleValue.arrValue[4];
            //rtapi_print("EndForceCond: the second argument retrieved!\n");
        }
        ++nArgIndex;
        if (bracketClosed)
            break;

        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 2, INTERP_ERR_SYNTAX_FAILED, _("EndForceCond function can only accept 2 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
    *counter = strlen(line);
    block->g_modes[GM_MODAL_0] = JK_SETENDFORCECONDITION;
    set_endforcecond_flag(1);
    return INTERP_OK;
}

int Interp::read_setforcectrlframe(char* line, int* counter, block_pointer block, double* parameters)
{
    if (_setup.defining_sub)
        return INTERP_OK;

    SYNCH_PARSE_EXEC;

    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);

    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        jaka_value eleValue;
        int subCounter = 0;

        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;

        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);

        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The  argument of setforcectrlframe can be a int value!"));
            block->ftFrame = eleValue.scalarValue;  // zero --> false, non-zero --> true
        }

        ++nArgIndex;
        if (bracketClosed)
            break;

        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 1, INTERP_ERR_SYNTAX_FAILED, _("setforcectrlframe function can only accept 1 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
    *counter = strlen(line);

    block->g_modes[GM_MODAL_0] = JK_SETFORCECONTROLFRAME;
    return INTERP_OK;
}

int Interp::read_disableforcecontrol(char* line, int* counter, block_pointer block, double* parameters)
{
    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;
    //printf("---------------------%c--------------\n",line[*counter]);
    CHKS((line[*counter] != ')'), INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
    *counter = strlen(line);
    // update the block motion type to JK_MOVL
    block->g_modes[0] = JK_DISABLEFORCECONTROL;
    return INTERP_OK;
}

int Interp::read_setvelcompliantlevel(char* line, int* counter, block_pointer block, double* parameters)
{
    if (_setup.defining_sub)
        return INTERP_OK;

    SYNCH_PARSE_EXEC;

    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);
    //rtapi_print("velCompliantCtrl: argsText is %s\n", argsText);

    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        jaka_value eleValue;
        int subCounter = 0;

        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;
        //rtapi_print("velCompliantCtrl: the text to read value: %s\n", arrEle);

        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);

        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of settool can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 5, INTERP_ERR_SYNTAX_FAILED, _("The velCompliantCtrl variable can only have 5 components!"));
            for (size_t i = 0; i < eleValue.arrValue.size(); ++i) block->velCompliantCtrl[i] = eleValue.arrValue[i];
            //rtapi_print("velCompliantCtrl: the first argument retrieved!\n");
        }

        ++nArgIndex;
        if (bracketClosed)
            break;

        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 1, INTERP_ERR_SYNTAX_FAILED, _("admitctrlconfig function can only accept 1 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
    *counter = strlen(line);
    block->g_modes[GM_MODAL_0] = JK_SETVELCOMPLIANTLEVEL;
    return INTERP_OK;
}

int Interp::read_setcompliantctrlcondition(char* line, int* counter, block_pointer block, double* parameters)
{
    if (_setup.defining_sub)
        return INTERP_OK;

    SYNCH_PARSE_EXEC;

    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);
    //rtapi_print("CompliantCondition: argsText is %s\n", argsText);

    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        jaka_value eleValue;
        int subCounter = 0;

        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;
        //rtapi_print("CompliantCondition: the text to read value: %s\n", arrEle);

        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);

        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of settool can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 6, INTERP_ERR_SYNTAX_FAILED, _("The compliantCondition variable can only have 4 components!"));
            for (size_t i = 0; i < eleValue.arrValue.size(); ++i) block->compliantCondition[i] = eleValue.arrValue[i];
            //rtapi_print("compliantCondition: the first argument retrieved!\n");
        }

        ++nArgIndex;
        if (bracketClosed)
            break;

        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 1, INTERP_ERR_SYNTAX_FAILED, _("compliantCondition function can only accept 1 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
    *counter = strlen(line);
    block->g_modes[GM_MODAL_0] = JK_SETCOMPLIANTCON;
    return INTERP_OK;
}

int Interp::read_setcomplianttype(char* line, int* counter, block_pointer block, double* parameters)
{
    /*command format: dout(index, tarState, bSynchronized)*/
    if (_setup.defining_sub)
        return INTERP_OK;

    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);

    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        jaka_value eleValue;
        int subCounter = 0;

        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;

        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);
        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of setdout can be only an integer!"));
            block->typeEna[0] = static_cast<int>(fabs(eleValue.scalarValue));
            //rtapi_print("block->typeEna[0] :  is %d\n", block->typeEna[0] );
        }
        else if (nArgIndex == 1)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of setdout can be only an integer!"));
            block->typeEna[1] = static_cast<int>(fabs(eleValue.scalarValue));
            //rtapi_print("block->typeEna[1] :  is %d\n", block->typeEna[1] );
        }

        ++nArgIndex;
        if (bracketClosed)
            break;

        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 2, INTERP_ERR_SYNTAX_FAILED, _("dout function can only accept 2 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
    *counter = strlen(line);

    // update the block motion type to JK_MOVL
    block->g_modes[GM_MODAL_0] = JK_SETCOMPLIANTTYPE;
    //rtapi_print("JK_SETCOMPLIANTTYPE\n" );
    return INTERP_OK;
}

/* make sure at least space bytes are available on

 * error channel; wait a bit to drain if needed
 */
int zucErrorBufferOKtoWrite2(int space, const char* caller, NML* zucErrBuffer)
{
    // check channel for validity
    if (zucErrBuffer == NULL)
        return -1;
    if (!zucErrBuffer->valid())
        return -1;

    double send_errorchan_timout = etime() + 1.0;

    while (etime() < send_errorchan_timout)
    {
        if (zucErrBuffer->get_space_available() < space)
        {
            // esleep(0.01);
            // continue;
            return -1;
        }
        else
        {
            break;
        }
    }
    if (etime() >= send_errorchan_timout)
    {
        // if (zuc_debug & ZUC_DEBUG_TASK_ISSUE)
        // {
        //     rcs_print("timeout waiting for error channel to drain, caller=`%s' request=%d\n", caller, space);
        // }
        return -1;
    }

    return 0;
}

std::string get_md5sum2(const std::string& filename)
{
    std::string md5string = "0";
    std::string cmd = "md5sum \"" + filename + "\" | cut -d ' ' -f 1";
    FILE* pipe = popen(cmd.c_str(), "r");
    if (pipe != NULL)
    {
        static const unsigned md5size = 32;
        unsigned char md5res[md5size + 1];
        int readSize = fread((void*)md5res, sizeof(char), md5size, pipe);
        pclose(pipe);
        if (readSize != md5size)
        {
            printf("Error reading md5 of %s\n", filename.c_str());
            return md5string;
        }
        md5res[md5size] = 0;
        md5string.assign(&md5res[0], &md5res[md5size]);
    }
    return md5string;
}

int Interp::read_setcollisionlevel(char* line, int* counter, block_pointer block, double* parameters)
{
    SYNCH_PARSE_EXEC;
    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;
    std::string msgString("null");
    _redis.getKeyValue("set_collision_level", msgString);
    // rtapi_print("[jks] redis message :%s  end.\n", msgString.c_str());
    if (atoi(msgString.c_str()) != 1)
    {
        std::string md5 = get_md5sum2(std::string(_setup.filename));
        const char dir[] = ZUC_SCRIPT_DIR;  //参考路径
        char filePath[PATH_MAX] = {0};
        strncpy(filePath, _setup.filename, PATH_MAX);
        if (strncmp(filePath, dir, strlen(dir)) == 0)
        {
            strncpy(filePath, &filePath[strlen(dir)], PATH_MAX - strlen(dir));  // 纪录相对于 ZUC_SCRIPT_DIR"/" 的文件路径。
        }
        rtapi_print("[jks] set collision level skip\n");
        char errString[512 + PATH_MAX] = {0};
        snprintf(errString,
                 511,
                 "set_collision_level Feature is not enabled #{\"fixed_key\":[\"%s\", \"%d\"],\"others\":{\"md5\":\"%s\"}}",
                 filePath,
                 _setup.sequence_number,
                 md5.c_str());
        rtapi_print("[jks] %s end \n", errString);
        zucLogError(INTERP_WARNING_DISABLEED_FUNCTION, errString);

        ZUC_OPERATOR_ERROR error_msg;
        error_msg.errcode = INTERP_WARNING_DISABLEED_FUNCTION;
        error_msg.error[0] = 0;
        if (0 != error_msg.errcode)
        {
            snprintf(error_msg.error, sizeof(error_msg.error), "[%d] %s", error_msg.errcode, errString);
        }
        error_msg.error[LINELEN - 1] = 0;
        // ERS(INTERP_WARNING_DISABLEED_FUNCTION,"FUCNTION set_collision NOT ENABLE");
        if (zucErrorBufferOKtoWrite2(sizeof(error_msg) * 2, "zucOperatorError", zucErrBuffer))
            return INTERP_ERROR;
        zucErrBuffer->write(error_msg);
        *counter = strlen(line);
        return INTERP_OK;
    }
    char argsText[LINELEN];
    strcpy(argsText, line + *counter);

    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        int subCounter = 0;
        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;

        jaka_value eleValue;
        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);

        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The  argument of setcollisionlevel can be only an integer value!"));
            block->utilNum = round_to_int(eleValue.scalarValue);
        }

        ++nArgIndex;
        if (bracketClosed)
            break;

        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 1, INTERP_ERR_SYNTAX_FAILED, _("setcollisionlevel function can only accept 1 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
    *counter = strlen(line);

    block->g_modes[GM_MODAL_0] = JK_SETCOLLISIONLEVEL;
    return INTERP_OK;
}

int Interp::read_logmsg(char* line, int* counter, block_pointer block, double* parameters)
{
    if (!MDImode)
    {
        SYNCH_PARSE_EXEC;
    }

    if (!parsing_flag && !beforeProgStartLine)
    {
        int msgType = 0;  // DEBUG: 0; INFO: 1; WARNING: 2; ERROR: 3; DELIMITER: 4;
        std::string msgString("");
        while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
        CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
        *counter = *counter + 1;
        char argsText[LINELEN];
        strcpy(argsText, line + *counter);

        int ret;
        int nArgIndex = 0;
        bool bracketClosed = false;
        const char* split = ",";
        char* arrEle = strtok(argsText, split);
        while (arrEle != NULL)
        {
            char* pchar = strstr(arrEle, ")");
            if (pchar != NULL)
            {  // contain the ')'
                bracketClosed = true;
                *pchar = '\0';
            }

            jaka_value eleValue;
            int subCounter = 0;
            while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
                ++subCounter;

            ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
            CHK_INTERP_READ(ret, line, counter);
            if (nArgIndex == 0)
            {
                CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of log_message can be only an integer value!"));
                msgType = round_to_int(eleValue.scalarValue);
                rtapi_print("log_message: the first argument is %d\n", msgType);
            }
            else if (nArgIndex == 1)
            {
                // CHKS(eleValue.valueType != STRING_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The second argument of log_message can be only an integer value!"));
                switch (eleValue.valueType)
                {
                case STRING_VALUE:
                    msgString = eleValue.strValue;
                    break;
                case SCALAR_VALUE:
                    msgString = std::to_string(eleValue.scalarValue);
                    break;
                case ARRAY_VALUE: {
                    msgString = "[";
                    int arrSize = eleValue.arrValue.size();
                    for (int i = 0; i < arrSize; i++)
                    {
                        msgString += std::to_string(eleValue.arrValue.at(i));
                        if (i < arrSize - 1)
                        {
                            msgString += ", ";
                        }
                    }
                    msgString = "]";
                }
                break;
                default:
                    ERS(INTERP_ERR_SYNTAX_FAILED, _("The second argument of log_message can be only an integer(or  an array or a string ) value!"));
                    break;
                }
                rtapi_print("log_message: the second argument is %s\nmessage_size=%d    maxSize: 256\n", msgString.c_str(), (int)msgString.length());
            }

            ++nArgIndex;
            if (bracketClosed)
                break;
            arrEle = strtok(NULL, split);
        }

        CHKS(nArgIndex != 2, INTERP_ERR_SYNTAX_FAILED, _("log_message function can only accept 2 arguments!"));
        CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
        if (msgType == 1)
        {
            zucLogInfo(0, msgString.c_str());
        }
        else if (msgType == 2)
        {
            zucLogWarning(0, msgString.c_str());
        }
        else if (msgType == 3)
        {
            zucLogError(0, msgString.c_str());
        }
    }

    *counter = strlen(line);
    return INTERP_OK;
}

int Interp::read_getsysclock(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    if (!MDImode)
    {
        SYNCH_PARSE_EXEC;
    }

    *counter = strlen(line);
    if (value_ptr == nullptr)
        return INTERP_OK;

    value_ptr->valueType = SCALAR_VALUE;
    value_ptr->scalarValue = 0;
    if (!parsing_flag)
    {
        struct timespec time1 = {0, 0};
        clock_gettime(CLOCK_MONOTONIC, &time1);
        double c = 1000000;
        value_ptr->scalarValue = time1.tv_sec * 1000 + time1.tv_nsec / c;
    }

    EXECUTING_BLOCK(_setup).m_modes[11] = GETSYSCLOCK;
    return INTERP_OK;
}

int Interp::read_pause(char* line, int* counter, block_pointer block, double* parameters)
{
    SYNCH_PARSE_EXEC;
    *counter = strlen(line);
    block->m_modes[5] = JK_PAUSE;
    // if (threadInterpList[0].thread_id)
    //     threadInterpList[0].pause_flag = 1;
    return INTERP_OK;
}

int Interp::read_resume(char* line, int* counter, block_pointer block, double* parameters)
{
    SYNCH_PARSE_EXEC;
    *counter = strlen(line);
    block->m_modes[5] = JK_RESUME;
    // if (threadInterpList[0].thread_id)
    //     threadInterpList[0].pause_flag = 1;
    return INTERP_OK;
}

int Interp::read_exit(char* line, int* counter, block_pointer block, double* parameters)
{
    SYNCH_PARSE_EXEC;
    *counter = strlen(line);
    block->m_modes[5] = JK_EXIT;
    return INTERP_OK;
}

int Interp::read_tioupdatesignal(char* line, int* counter, block_pointer block, double* parameters)
{
    SYNCH_PARSE_EXEC;

    if (beforeProgStartLine)
    {
        *counter = strlen(line);
        return INTERP_OK;
    }

    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);

    int ret, nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        jaka_value eleValue;
        int subCounter = 0;
        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;
        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);
        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != STRING_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of tioupdatesignal can an interger!"));
            strcpy(block->comment, eleValue.strValue.c_str());
        }
        else if (nArgIndex == 1)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The second argument of tioupdatesignal can be only an array variable!"));
            block->utilFloat = round_to_int(eleValue.scalarValue);
        }
        ++nArgIndex;
        if (bracketClosed)
            break;
        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 2, INTERP_ERR_SYNTAX_FAILED, _("tio_update_signal function can only accept 2 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
    *counter = strlen(line);
    block->g_modes[GM_MODAL_0] = JK_TIO_UPDATE_SIGNAL;
    return INTERP_OK;
}

int Interp::read_tiosendcommand(char* line, int* counter, block_pointer block, double* parameters)
{
    SYNCH_PARSE_EXEC;
    if (beforeProgStartLine)
    {
        *counter = strlen(line);
        return INTERP_OK;
    }

    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);

    int ret, nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        jaka_value eleValue;
        int subCounter = 0;
        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;
        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);
        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of tiosendcommand can an interger!"));
            block->utilId = round_to_int(eleValue.scalarValue);
        }
        else if (nArgIndex == 1)
        {
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The second argument of tiosendcommand can be only an array variable!"));
            for (int i = 0; i < (int)eleValue.arrValue.size() && i < 32; i++) { block->comment[i] = (round_to_int(eleValue.arrValue[i]) & 0xFF); }
            block->utilNum = (eleValue.arrValue.size() < 32 ? eleValue.arrValue.size() : 32);
        }
        else if (nArgIndex == 2)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The third argument of tiosendcommand can be only an integer variable!"));
            block->utilType = round_to_int(eleValue.scalarValue);
        }
        ++nArgIndex;
        if (bracketClosed)
            break;
        arrEle = strtok(NULL, split);
    }
    CHKS(nArgIndex != 2 && nArgIndex != 3, INTERP_ERR_SYNTAX_FAILED, _("tio_send_command function can only accept 2 or 3 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));

    *counter = strlen(line);
    block->g_modes[GM_MODAL_0] = JK_TIO_SEND_COMMAND;
    return INTERP_OK;
}

int Interp::read_tiogetsignalvalue(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    SYNCH_PARSE_EXEC;
    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);

    int ret, nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);

    std::string signalName("");
    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        jaka_value eleValue;
        int subCounter = 0;
        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;
        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);
        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != STRING_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of tio_get_signal_value can a string value!"));
            signalName = eleValue.strValue;
        }
        ++nArgIndex;
        if (bracketClosed)
            break;
        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 1, INTERP_ERR_SYNTAX_FAILED, _("tio_get_signal_value function can only accept 1 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
    *counter = strlen(line);

    unsigned int sigValue = 0;
    if (usrmotGetTIOSignalValue(signalName.c_str(), &sigValue) == -1)
    {
        ERS(INTERP_ERR_READ_TIO_SIGNAL_FAILED, "read tio signal failed, undefined signal %s", signalName.c_str());
    }
    else if (usrmotGetTIOSignalValue(signalName.c_str(), &sigValue) == 1)
    {
        ERS(INTERP_ERR_TIO_SIGNAL_MISS, "read tio signal failed, read signal %s timeout\n", signalName.c_str());
    }

    if (value_ptr)
    {
        value_ptr->valueType = SCALAR_VALUE;
        value_ptr->scalarValue = sigValue;
    }

    EXECUTING_BLOCK(_setup).m_modes[11] = GETTIOSIGNALVAL;
    return INTERP_OK;
}

int Interp::read_threadcreate(char* line, int* counter, block_pointer block, double* parameters)
{
    SYNCH_PARSE_EXEC;
    if (threadInterpNum >= MAX_SUB_THREAD)
    {
        rtapi_print("interpreter error:thread number(%d) over limit (MAX_SUB_THREAD_NUM = %d)", threadInterpNum, MAX_SUB_THREAD);
        ERS(INTERP_ERR_TOO_MANY_THREAD, "Too many thread has been created. ");
    }
    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);

    std::string threadFuncFile("");
    std::string threadAlias("");
    int ret, nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        jaka_value eleValue;
        int subCounter = 0;
        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;
        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);
        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != STRING_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of create_thread must be  a string value!"));
            threadFuncFile = eleValue.strValue;
        }
        else if (nArgIndex == 1)
        {
            CHKS(eleValue.valueType != STRING_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The second argument of create_thread must be a string value!"));
            threadAlias = eleValue.strValue;
        }
        ++nArgIndex;
        if (bracketClosed)
            break;
        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 2, INTERP_ERR_SYNTAX_FAILED, _("create_thread function can only accept 2 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
    *counter = strlen(line);

    int index = 0;
    for (; index < MAX_SUB_THREAD; index++)
    {
        if (threadAlias.size() == 0 || threadInterpList[index].thread_alias.compare(threadAlias) == 0)
        {
            rtapi_print("thread_name should be a unique string. (not empty) \n");
            ERS(INTERP_ERR_THREAD_IS_RUNNING, "Create thread %s failed, thread %s is running", threadAlias.c_str(), threadAlias.c_str());
        }
        else if (threadInterpList[index].thread_alias.size() == 0)
        {
            break;
        }
    }
    if (index >= MAX_SUB_THREAD)
    {
        rtapi_print("interpreter error:thread number over limit (MAX_SUB_THREAD_NUM = %d)", MAX_SUB_THREAD);
        ERS(INTERP_ERR_TOO_MANY_THREAD, "Too many thread has been created.");
    }

    if (threadInterpList[index].is_running == false)
    {
        std::shared_ptr<MultiThreadInterp> pInterp(new MultiThreadInterp(index));
        memcpy(pInterp->_readers, default_readers, sizeof(default_readers));
        pInterp->close();
        pInterp->_setup.parameters = _setup.parameters;
        ret = pInterp->open(threadFuncFile.c_str());
        if (ret != INTERP_OK)
        {
            return ret;
        }
        threadInterpList[index].threadInterp = pInterp;
        threadInterpList[index].nmlcmd_list.reset(new NML_INTERP_LIST());
    }
    else
    {
        // threadInterpList[index].threadInterp->init();     // init() 会导致脚本编程中的系统变量被重置。
        threadInterpList[index].threadInterp->close();
        ((MultiThreadInterp*)threadInterpList[index].threadInterp.get())->_setup.parameters = _setup.parameters;
        ret = threadInterpList[index].threadInterp->open(threadFuncFile.c_str());
        if (ret != INTERP_OK)
        {
            return ret;
        }
        threadInterpList[index].threadInterp->_waitFlag = false;
        threadInterpList[index].auxInputWaitMsg.waiting = false;
        threadInterpList[index].threadInterp->set_thread_id(index + 1);
    }

    AuxInputWaitMsg auxInputWait;
    threadInterpList[index].thread_id = index + 1;
    threadInterpList[index].pth_t = 0;
    threadInterpList[index].auxInputWaitMsg = auxInputWait;
    threadInterpList[index].thread_alias = threadAlias;
    threadInterpList[index].new_flag = 1;
    threadInterpList[index].destroy_flag = 0;
    threadInterpList[index].pause_flag = 0;
    threadInterpList[index].join_flag = 0;
    threadInterpList[index].destroied_flag = 0;
    // threadInterpList[index].is_mutex = 0;
    // threadInterpList[index].is_running = 0;
    threadInterpNum++;

    rtapi_print("thread created id>>>>>: %d  name : %s    index:  %d \n", index, threadAlias.c_str(), threadInterpNum);
    rtapi_print("thread created  >>>>>, file opened: %s\n", threadFuncFile.c_str());
    // ++tid;

    return INTERP_OK;
}

int Interp::read_threaddestroy(char* line, int* counter, block_pointer block, double* parameters)
{
    SYNCH_PARSE_EXEC;
    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);
    int ret, nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    std::string thread_alias("");

    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        jaka_value eleValue;
        int subCounter = 0;
        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;
        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);
        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != STRING_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of tio_get_signal_value can a string value!"));
            thread_alias = eleValue.strValue;
        }
        ++nArgIndex;
        if (bracketClosed)
            break;
        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 1, INTERP_ERR_SYNTAX_FAILED, _("tio_get_signal_value function can only accept 1 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
    *counter = strlen(line);

    for (int i = 0; i < MAX_SUB_THREAD; ++i)
    {
        if (threadInterpList[i].thread_alias.compare(thread_alias) == 0)
        {
            threadInterpList[i].destroy_flag = 1;
            return INTERP_OK;
        }
    }
    rtapi_print("Error-Destroy: Thread %s not Found\n", thread_alias.c_str());
    return INTERP_OK;
}

int Interp::read_threadjoin(char* line, int* counter, block_pointer block, double* parameters)
{
    SYNCH_PARSE_EXEC;
    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);

    int ret, nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    std::string thread_alias("");

    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        jaka_value eleValue;
        int subCounter = 0;
        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;
        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);
        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != STRING_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of thread_join can a string value!"));
            thread_alias = eleValue.strValue;
        }
        ++nArgIndex;
        if (bracketClosed)
            break;
        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 1, INTERP_ERR_SYNTAX_FAILED, _("thread_join function can only accept 1 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
    *counter = strlen(line);

    for (int i = 0; i < MAX_SUB_THREAD; ++i)
    {
        if (threadInterpList[i].thread_alias.compare(thread_alias) == 0)
        {
            threadInterpList[i].join_flag = 1;
            SYNCH_PARSE_EXEC;
            return INTERP_OK;
        }
    }
    rtapi_print("Error-Join: Thread %s not Found\n", thread_alias.c_str());

    return INTERP_OK;
}

int Interp::read_threadentermutex(char* line, int* counter, block_pointer block, double* parameters)
{
    SYNCH_PARSE_EXEC;
    for (int i = 0; i < MAX_SUB_THREAD; ++i) { threadInterpList[i].is_mutex = 1; }
    is_mutex = 1;

    return INTERP_OK;
}

int Interp::read_threadexitmutex(char* line, int* counter, block_pointer block, double* parameters)
{
    SYNCH_PARSE_EXEC;
    for (int i = 0; i < MAX_SUB_THREAD; ++i) { threadInterpList[i].is_mutex = 0; }
    is_mutex = 0;

    return INTERP_OK;
}
/****************************************************************************/

/*! read_b

 Returned Value: int
 If read_real_value returns an error code, this returns that code.
 If any of the following errors occur, this returns the error code shown.
 Otherwise, it returns INTERP_OK.
 1. The first character read is not b:
 NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED
 2. A b_coordinate has already been inserted in the block:
 NCE_MULTIPLE_B_WORDS_ON_ONE_LINE.
 3. B values are not allowed: NCE_CANNOT_USE_B_WORD

 Side effects:
 counter is reset.
 The b_flag in the block is turned on.
 A b_number is inserted in the block.

 Called by: read_one_item

 When this function is called, counter is pointing at an item on the
 line that starts with the character 'b', indicating a b_coordinate
 setting. The function reads characters which tell how to set the
 coordinate, up to the start of the next item or the end of the line.
 The counter is then set to point to the character following.

 The value may be a real number or something that evaluates to a
 real number, so read_real_value is used to read it. Parameters
 may be involved.

 If the BB compiler flag is defined, the b_flag in the block is turned
 on and the b_number in the block is set to the value read. If the
 BB flag is not defined, (i) if the AXIS_ERROR flag is defined, that means
 B values are not allowed, and an error value is returned, (ii) if the
 AXIS_ERROR flag is not defined, nothing is done.

 */

int Interp::read_b(char* line,           //!< string: line of RS274/NGC code being processed
                   int* counter,         //!< pointer to a counter for position on the line
                   block_pointer block,  //!< pointer to a block being filled from the line
                   double* parameters)   //!< array of system parameters
{
    jaka_value value;

    CHKS((line[*counter] != 'b'), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = (*counter + 1);
    CHKS((block->b_flag), INTERP_ERR_SYNTAX_FAILED, NCE_MULTIPLE_B_WORDS_ON_ONE_LINE);
    CHP(read_real_value(line, counter, &value, parameters));
    block->b_flag = true;
    block->b_number = value.scalarValue;
    return INTERP_OK;
}

/****************************************************************************/

/*! read_c

 Returned Value: int
 If read_real_value returns an error code, this returns that code.
 If any of the following errors occur, this returns the error code shown.
 Otherwise, it returns INTERP_OK.
 1. The first character read is not c:
 NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED
 2. An c_coordinate has already been inserted in the block:
 NCE_MULTIPLE_C_WORDS_ON_ONE_LINE
 3. C values are not allowed: NCE_CANNOT_USE_C_WORD

 Side effects:
 counter is reset.
 The c_flag in the block is turned on.
 A c_number is inserted in the block.

 Called by: read_one_item

 When this function is called, counter is pointing at an item on the
 line that starts with the character 'c', indicating an c_coordinate
 setting. The function reads characters which tell how to set the
 coordinate, up to the start of the next item or the end of the line.
 The counter is then set to point to the character following.

 The value may be a real number or something that evaluates to a
 real number, so read_real_value is used to read it. Parameters
 may be involved.

 If the CC compiler flag is defined, the c_flag in the block is turned
 on and the c_number in the block is set to the value read. If the
 CC flag is not defined, (i) if the AXIS_ERROR flag is defined, that means
 C values are not allowed, and an error value is returned, (ii) if the
 AXIS_ERROR flag is not defined, nothing is done.

 */

int Interp::read_c(char* line,           //!< string: line of RS274/NGC code being processed
                   int* counter,         //!< pointer to a counter for position on the line
                   block_pointer block,  //!< pointer to a block being filled from the line
                   double* parameters)   //!< array of system parameters
{
    jaka_value value;

    CHKS((line[*counter] != 'c'), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = (*counter + 1);
    CHKS((block->c_flag), INTERP_ERR_SYNTAX_FAILED, NCE_MULTIPLE_C_WORDS_ON_ONE_LINE);
    CHP(read_real_value(line, counter, &value, parameters));
    block->c_flag = true;
    block->c_number = value.scalarValue;
    return INTERP_OK;
}

/****************************************************************************/

/*! read_comment

 Returned Value: int
 If any of the following errors occur, this returns the error code shown.
 Otherwise, it returns INTERP_OK.
 1. The first character read is not '//' ,
 NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED

 Side effects:
 The counter is reset to point to the character following the comment.
 The comment string, without parentheses, is copied into the comment
 area of the block.

 Called by: read_one_item

 When this function is called, counter is pointing at an item on the
 line that starts with the character '//', indicating a comment is
 beginning. The function reads characters of the comment, until to the 
 end of the line.

 It is expected that the format of a comment will have been checked (by
 read_text or read_keyboard_line) and bad format comments will
 have prevented the system from getting this far, so that this function
 can assume a close parenthesis will be found when an open parenthesis
 has been found, and that comments are not nested.

 The "parameters" argument is not used in this function. That argument is
 present only so that this will have the same argument list as the other
 "read_XXX" functions called using a function pointer by read_one_item.

 */

int Interp::read_comment(char* line,           //!< string: line of RS274 code being processed
                         int* counter,         //!< pointer to a counter for position on the line
                         block_pointer block,  //!< pointer to a block being filled from the line
                         double* parameters)   //!< array of system parameters
{
    int n;

    // '//' indicates the start of a line of comment
    CHKS((line[*counter] != '#'), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    (*counter) += 1;  // skip '#'
    for (n = 0; *counter < (int)strlen(line); (*counter)++, n++) { block->comment[n] = line[*counter]; }
    block->comment[n] = 0;
    (*counter)++;
    return INTERP_OK;
}

// A semicolon marks the beginning of a comment.  The comment goes to
// the end of the line.

int Interp::read_semicolon(char* line,           //!< string: line of RS274 code being processed
                           int* counter,         //!< pointer to a counter for position on the line
                           block_pointer block,  //!< pointer to a block being filled from the line
                           double* parameters)   //!< array of system parameters
{
    char* s;
    CHKS((line[*counter] != ';'), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    (*counter) = strlen(line);
    // pass unmutilated line to convert_comment - FIXME access to _setup
    if ((s = strchr(_setup.linetext, ';')) != NULL)
        CHP(convert_comment(s + 1, false));
    return INTERP_OK;
}

/****************************************************************************/

/*! read_d

 Returned Value: int
 If read_integer_value returns an error code, this returns that code.
 If any of the following errors occur, this returns the error code shown.
 Otherwise, it returns INTERP_OK.
 1. The first character read is not d:
 NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED
 2. A d_number has already been inserted in the block:
 NCE_MULTIPLE_D_WORDS_ON_ONE_LINE

 Side effects:
 counter is reset to the character following the tool number.
 A d_number is inserted in the block.

 Called by: read_one_item

 When this function is called, counter is pointing at an item on the
 line that starts with the character 'd', indicating an index into a
 table of tool diameters.  The function reads characters which give the
 (integer) value of the index. The value may not be more than
 _setup.tool_max and may not be negative, but it may be zero. The range
 is checked here.

 read_integer_value allows a minus sign, so a check for a negative value
 is made here, and the parameters argument is also needed.

 */

int Interp::read_d(char* line,           //!< string: line of RS274 code being processed
                   int* counter,         //!< pointer to a counter for position on the line
                   block_pointer block,  //!< pointer to a block being filled from the line
                   double* parameters)   //!< array of system parameters
{
    jaka_value value;

    CHKS((line[*counter] != 'd'), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = (*counter + 1);
    CHKS((block->d_flag), INTERP_ERR_SYNTAX_FAILED, NCE_MULTIPLE_D_WORDS_ON_ONE_LINE);
    CHP(read_real_value(line, counter, &value, parameters));
    block->d_number_float = value.scalarValue;
    block->d_flag = true;
    return INTERP_OK;
}

/****************************************************************************/

/*! read_e

 Returned Value: int
 If read_real_value returns an error code, this returns that code.
 If any of the following errors occur, this returns the error code shown.
 Otherwise, it returns INTERP_OK.
 1. The first character read is not e:
 NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED
 2. A e value has already been inserted in the block:
 NCE_MULTIPLE_E_WORDS_ON_ONE_LINE

 Side effects:
 counter is reset to point to the first character following the e value.
 The e value setting is inserted in block.

 Called by: read_one_item

 When this function is called, counter is pointing at an item on the
 line that starts with the character 'e', indicating a e value
 setting. The function reads characters which tell how to set the e
 value, up to the start of the next item or the end of the line. This
 information is inserted in the block.

 E codes are used for:
 Infeed/Outfeed angle specification with G76

 */

int Interp::read_e(char* line,           //!< string: line of RS274/NGC code being processed
                   int* counter,         //!< pointer to a counter for position on the line
                   block_pointer block,  //!< pointer to a block being filled from the line
                   double* parameters)   //!< array of system parameters
{
    jaka_value value;

    CHKS((line[*counter] != 'e'), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = (*counter + 1);
    CHKS((block->e_flag), INTERP_ERR_SYNTAX_FAILED, NCE_MULTIPLE_E_WORDS_ON_ONE_LINE);
    CHP(read_real_value(line, counter, &value, parameters));
    block->e_flag = true;
    block->e_number = value.scalarValue;
    return INTERP_OK;
}

/****************************************************************************/

/*! read_f

 Returned Value: int
 If read_real_value returns an error code, this returns that code.
 If any of the following errors occur, this returns the error code shown.
 Otherwise, it returns INTERP_OK.
 1. The first character read is not f:
 NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED
 2. An f_number has already been inserted in the block:
 NCE_MULTIPLE_F_WORDS_ON_ONE_LINE
 3. The f_number is negative: NCE_NEGATIVE_F_WORD_USED

 Side effects:
 counter is reset to point to the first character following the f_number.
 The f_number is inserted in block and the f_flag is set.

 Called by: read_one_item

 When this function is called, counter is pointing at an item on the
 line that starts with the character 'f'. The function reads characters
 which tell how to set the f_number, up to the start of the next item
 or the end of the line. This information is inserted in the block.

 The value may be a real number or something that evaluates to a real
 number, so read_real_value is used to read it. Parameters may be
 involved, so the parameters argument is required. The value is always
 a feed rate.

 */

int Interp::read_f(char* line,           //!< string: line of RS274 code being processed
                   int* counter,         //!< pointer to a counter for position on the line
                   block_pointer block,  //!< pointer to a block being filled from the line
                   double* parameters)   //!< array of system parameters
{
    jaka_value value;

    CHKS((line[*counter] != 'f'), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = (*counter + 1);
    CHKS((block->f_flag), INTERP_ERR_SYNTAX_FAILED, NCE_MULTIPLE_F_WORDS_ON_ONE_LINE);
    CHP(read_real_value(line, counter, &value, parameters));
    CHKS((value.scalarValue < 0.0), INTERP_ERR_SYNTAX_FAILED, NCE_NEGATIVE_F_WORD_USED);
    block->f_number = value.scalarValue;
    block->f_flag = true;
    return INTERP_OK;
}

/****************************************************************************/

/*! read_g

 Returned Value: int
 If read_real_value returns an error code, this returns that code.
 If any of the following errors occur, this returns the error code shown.
 Otherwise, it returns INTERP_OK.
 1. The first character read is not g:
 NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED
 2. The value is negative: NCE_NEGATIVE_G_CODE_USED
 3. The value differs from a number ending in an even tenth by more
 than 0.0001: NCE_G_CODE_OUT_OF_RANGE
 4. The value is greater than 99.9: NCE_G_CODE_OUT_OF_RANGE
 5. The value is not the number of a valid g code: NCE_UNKNOWN_G_CODE_USED
 6. Another g code from the same modal group has already been
 inserted in the block: NCE_TWO_G_CODES_USED_FROM_SAME_MODAL_GROUP

 Side effects:
 counter is reset to the character following the end of the g_code.
 A g code is inserted as the value of the appropriate mode in the
 g_modes array in the block.
 The g code counter in the block is increased by 1.

 Called by: read_one_item

 When this function is called, counter is pointing at an item on the
 line that starts with the character 'g', indicating a g_code.  The
 function reads characters which tell how to set the g_code.

 The RS274/NGC manual [NCMS, page 51] allows g_codes to be represented
 by expressions and provide [NCMS, 71 - 73] that a g_code must evaluate
 to to a number of the form XX.X (59.1, for example). The manual does not
 say how close an expression must come to one of the allowed values for
 it to be legitimate. Is 59.099999 allowed to mean 59.1, for example?
 In the interpreter, we adopt the convention that the evaluated number
 for the g_code must be within 0.0001 of a value of the form XX.X

 To simplify the handling of g_codes, we convert them to integers by
 multiplying by 10 and rounding down or up if within 0.001 of an
 integer. Other functions that deal with g_codes handle them
 symbolically, however. The symbols are defined in rs274NGC.hh
 where G_1 is 10, G_83 is 830, etc.

 This allows any number of g_codes on one line, provided that no two
 are in the same modal group.

 This allows G80 on the same line as one other g_code with the same
 mode. If this happens, the G80 is simply ignored.

 */

int Interp::read_g(char* line,           //!< string: line of RS274/NGC code being processed
                   int* counter,         //!< pointer to a counter for position on the line
                   block_pointer block,  //!< pointer to a block being filled from the line
                   double* parameters)   //!< array of system parameters
{
    jaka_value value_read;
    int value;
    int mode;

    CHKS((line[*counter] != 'g'), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = (*counter + 1);
    CHP(read_real_value(line, counter, &value_read, parameters));
    value_read.scalarValue = (10.0 * value_read.scalarValue);
    value = (int)floor(value_read.scalarValue);

    if ((value_read.scalarValue - value) > 0.999)
        value = (int)ceil(value_read.scalarValue);
    else if ((value_read.scalarValue - value) > 0.001)
        ERS(INTERP_ERR_INTERNAL, NCE_G_CODE_OUT_OF_RANGE);

    CHKS((value > 999), INTERP_ERR_INTERNAL, NCE_G_CODE_OUT_OF_RANGE);
    CHKS((value < 0), INTERP_ERR_SYNTAX_FAILED, NCE_NEGATIVE_G_CODE_USED);
    // mode = usercode_mgroup(&(_setup),value);
    // if (mode != -1) {

    remap_pointer r = _setup.g_remapped[value];
    if (r)
    {
        mode = r->modal_group;
        CHKS((mode < 0), INTERP_ERR_SYNTAX_FAILED, "BUG: G remapping: modal group < 0");  // real bad

        CHKS((block->g_modes[mode] != -1), INTERP_ERR_SYNTAX_FAILED, NCE_TWO_G_CODES_USED_FROM_SAME_MODAL_GROUP);
        block->g_modes[mode] = value;
        return INTERP_OK;
    }
    mode = _gees[value];
    CHKS((mode == -1), INTERP_ERR_SYNTAX_FAILED, NCE_UNKNOWN_G_CODE_USED);
    if ((value == G_80) && (block->g_modes[mode] != -1))
        ;
    else
    {
        if (block->g_modes[mode] == G_80)
            ;
        else
        {
            CHKS((block->g_modes[mode] != -1), INTERP_ERR_SYNTAX_FAILED, NCE_TWO_G_CODES_USED_FROM_SAME_MODAL_GROUP);
        }
        block->g_modes[mode] = value;
    }
    return INTERP_OK;
}

int Interp::read_movj(char* line, int* counter, block_pointer block, double* parameters)
{
    if (_setup.defining_sub)
        return INTERP_OK;

    if (endForceCondFlag == 2)
    {
        rtapi_print("endForceCondFlag------: %d, %s\n", endForceCondFlag, line);
        set_endforcecond_flag(0);
        SYNCH_PARSE_EXEC;
    }
    // skip the space character, and check the '(' token
    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    End_Cond defCond = {-1, -1, 0};  // disabled by default
    block->condition = defCond;

    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char argsText[LINELEN];
    strcpy(argsText, line + *counter);
    char* arrEle = strtok(argsText, split);
    int jnum = get_joint_num();  //for atom

    block->planner_type = -1;
    block->robot_id = 0;  // 默认机器人1

    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        jaka_value eleValue;
        int subCounter = 0;

        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;

        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);

        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of movj can be only an array variable!"));
            if (jnum == 7)
            {
                // @FIXME liwang 临时使得7轴可运行6轴的程序
                // if (eleValue.arrValue.size() != 7)
                // {
                //     eleValue.arrValue.push_back(0);
                // }
            }
            else
            {
                CHKS(eleValue.arrValue.size() != 6, INTERP_ERR_SYNTAX_FAILED, _("The position variable can only have 6 components!"));
            }

            for (size_t i = 0; i < eleValue.arrValue.size(); ++i) block->cmd_pos[i] = eleValue.arrValue[i];
        }
        else if (nArgIndex == 1)
        {  // relative motion or not
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The second argument of movj can be only 0 or 1!"));
            block->rel_motion_flag = (fabs(eleValue.scalarValue) >= 1e-6);  // zero --> false, non-zero --> true
        }
        else if (nArgIndex == 2)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The third argument of movj can be only a scalar value!"));
            CHKS((eleValue.scalarValue < 0.0), INTERP_ERR_SYNTAX_FAILED, _("The velocity of movj cannot be a negative value!"));
            block->cmd_vel = eleValue.scalarValue;
        }
        else if (nArgIndex == 3)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The forth argument of movj can be only a scalar value!"));
            CHKS((eleValue.scalarValue < 0.0), INTERP_ERR_SYNTAX_FAILED, _("The acceleration cannot be a negative value!"));
            block->acc = eleValue.scalarValue;
        }
        else if (nArgIndex == 4)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The fifth argument of movj can be only a scalar value!"));
            CHKS((eleValue.scalarValue < 0.0), INTERP_ERR_SYNTAX_FAILED, _("The tolerance cannot be a negative value!"));
            block->tolerance = eleValue.scalarValue;
        }
        else if (nArgIndex == 5)
        {  // optional arguments
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The 7th argument of movj can be only a array value!"));
            CHKS(eleValue.arrValue.size() != 3, INTERP_ERR_SYNTAX_FAILED, _("The condition variable can only have 3 components!"));
            block->condition.di_type = eleValue.arrValue[0];
            block->condition.di_index = eleValue.arrValue[1];
            block->condition.di_state = eleValue.arrValue[2];
        }
        else if (nArgIndex == 6)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The 7th argument of movj can be only a scalar value!"));
            CHKS((eleValue.scalarValue < 0.0), INTERP_ERR_SYNTAX_FAILED, _("The planner cannot be a negative value!"));
            block->planner_type = eleValue.scalarValue;
        }
        else if (nArgIndex == 7)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The 7th argument of movj can be only a scalar value!"));
            CHKS((eleValue.scalarValue < 0.0), INTERP_ERR_SYNTAX_FAILED, _("The planner cannot be a negative value!"));
            block->robot_id = eleValue.scalarValue;
        }
        ++nArgIndex;
        if (bracketClosed)
            break;

        arrEle = strtok(NULL, split);
    }

    int mtn_end_cond = (block->condition.di_type >= 0 && block->condition.di_index >= 0);
    if (block->last_mtn_end_cond)
    {
        block->last_mtn_end_cond = mtn_end_cond;
        SYNCH_PARSE_EXEC;
    }
    block->last_mtn_end_cond = mtn_end_cond;

    CHKS(nArgIndex != 5 && nArgIndex != 6 && nArgIndex != 7 && nArgIndex != 8,
         INTERP_ERR_SYNTAX_FAILED,
         _("movj function can only accept 4 or 5 or 6 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
    *counter = strlen(line);
    if (block->planner_type == -1 && nArgIndex == 5)
    {
        // 智元兼容171APP，默认采用T规划
        block->planner_type = 0;
    }
    // update the block motion type to JK_MOVL
    block->g_modes[1] = JK_MOVJ;
    if (endForceCondFlag == 1)
    {
        rtapi_print("endForceCondFlag------: %d, %s\n", endForceCondFlag, line);
        set_endforcecond_flag(2);
    }
    return INTERP_OK;
}

int Interp::read_movl(char* line, int* counter, block_pointer block, double* parameters)
{
    /*command format: movl(#<postTo>, rel/abs, vel)*/
    if (_setup.defining_sub)
        return INTERP_OK;

    if (endForceCondFlag == 2)
    {
        rtapi_print("endForceCondFlag------: %d, %s\n", endForceCondFlag, line);
        set_endforcecond_flag(0);
        SYNCH_PARSE_EXEC;
    }
    // skip the space character, and check the '(' token
    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    End_Cond defCond = {-1, -1, 0};  // disabled by default
    block->condition = defCond;

    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char argsText[LINELEN];
    strcpy(argsText, line + *counter);
    char* arrEle = strtok(argsText, split);
    block->ori_vel = 180.0;
    block->ori_acc = 720.0;  //need to be limited later
    block->planner_type = -1;
    block->robot_id = 0;

    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        jaka_value eleValue;
        int subCounter = 0;

        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;

        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);

        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of movl can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 6, INTERP_ERR_SYNTAX_FAILED, _("The position variable can only have 6 components!"));
            for (size_t i = 0; i < eleValue.arrValue.size(); ++i) block->cmd_pos[i] = eleValue.arrValue[i];
        }
        else if (nArgIndex == 1)
        {  // relative motion or not
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The second argument of movl can be only 0 or 1!"));
            block->rel_motion_flag = eleValue.scalarValue;  // zero --> false, non-zero --> true
                                                            //rtapi_print(" ================== eleValue.scalarValue = %f \n",eleValue.scalarValue);
        }
        else if (nArgIndex == 2)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The third argument of movl can be only a scalar value!"));
            CHKS((eleValue.scalarValue < 0.0), INTERP_ERR_SYNTAX_FAILED, _("The velocity of movl cannot be a negative value!"));
            block->cmd_vel = eleValue.scalarValue;
        }
        else if (nArgIndex == 3)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The forth argument of movl can be only a scalar value!"));
            CHKS((eleValue.scalarValue < 0.0), INTERP_ERR_SYNTAX_FAILED, _("The acceleration cannot be a negative value!"));
            block->acc = eleValue.scalarValue;
        }
        else if (nArgIndex == 4)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The fifth argument of movl can be only a scalar value!"));
            CHKS((eleValue.scalarValue < 0.0), INTERP_ERR_SYNTAX_FAILED, _("The tolerance cannot be a negative value!"));
            block->tolerance = eleValue.scalarValue;
        }
        else if (nArgIndex == 5)
        {  // optional argument
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The ninth argument of movl can be only a array value!"));
            CHKS(eleValue.arrValue.size() != 3, INTERP_ERR_SYNTAX_FAILED, _("The condition variable can only have 3 components!"));
            block->condition.di_type = eleValue.arrValue[0];
            block->condition.di_index = eleValue.arrValue[1];
            block->condition.di_state = eleValue.arrValue[2];
        }
        else if (nArgIndex == 6)
        {  // optional argument
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The sixth argument of movl can be only a scalar value!"));
            CHKS((eleValue.scalarValue < 0.0), INTERP_ERR_SYNTAX_FAILED, _("The velocity of movl cannot be a negative value!"));
            block->ori_vel = eleValue.scalarValue;
        }
        else if (nArgIndex == 7)
        {  // optional argument
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The seventh argument of movl can be only a scalar value!"));
            CHKS((eleValue.scalarValue < 0.0), INTERP_ERR_SYNTAX_FAILED, _("The acceleration cannot be a negative value!"));
            block->ori_acc = eleValue.scalarValue;
        }
        else if (nArgIndex == 8)
        {  // optional argument
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The eighth argument of movj can be only a scalar value!"));
            CHKS((eleValue.scalarValue < 0.0), INTERP_ERR_SYNTAX_FAILED, _("The planner cannot be a negative value!"));
            block->planner_type = eleValue.scalarValue;
        }
        else if (nArgIndex == 9)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The 7th argument of movj can be only a scalar value!"));
            CHKS((eleValue.scalarValue < 0.0), INTERP_ERR_SYNTAX_FAILED, _("The planner cannot be a negative value!"));
            block->robot_id = eleValue.scalarValue;
        }
        ++nArgIndex;
        if (bracketClosed)
            break;

        arrEle = strtok(NULL, split);
    }
    // rtapi_print("----------------ori_vel: %lf, ori_acc: %lf-------------\n", block->ori_vel, block->ori_acc);

    int mtn_end_cond = (block->condition.di_type >= 0 && block->condition.di_index >= 0);
    if (block->last_mtn_end_cond)
    {
        block->last_mtn_end_cond = mtn_end_cond;
        SYNCH_PARSE_EXEC;
    }
    block->last_mtn_end_cond = mtn_end_cond;
    if (block->planner_type == -1 && nArgIndex == 8)
    {
        // 默认使用T型规划
        block->planner_type = 0;
    }
    CHKS(nArgIndex < 5 || nArgIndex > 10, INTERP_ERR_SYNTAX_FAILED, _("movl function can only accept 8 or 9 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
    *counter = strlen(line);

    // update the block motion type to JK_MOVL
    block->g_modes[1] = JK_MOVL;

    if (endForceCondFlag == 1)
    {
        rtapi_print("endForceCondFlag------: %d, %s\n", endForceCondFlag, line);
        set_endforcecond_flag(2);
    }

    return INTERP_OK;
}

/*added by zxqi to support parsing of the movc motion*/
int Interp::read_movc(char* line, int* counter, block_pointer block, double* parameters)
{
    /*command format: movl(#<postTo>, rel/abs, vel)*/
    if (_setup.defining_sub)
        return INTERP_OK;

    if (endForceCondFlag == 2)
    {
        rtapi_print("endForceCondFlag------: %d, %s\n", endForceCondFlag, line);
        set_endforcecond_flag(0);
        SYNCH_PARSE_EXEC;
    }
    // skip the space character, and check the '(' token
    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    // printf( "--------read_movc function------: counter =%d   c = %c  \n  ", *counter,line[*counter]);
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    End_Cond defCond = {-1, -1, 0};  // disabled by default
    block->condition = defCond;
    block->count = 0.0;
    block->movcmode = 0;  //movc mid mode init
    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char argsText[LINELEN];
    strcpy(argsText, line + *counter);
    char* arrEle = strtok(argsText, split);
    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        jaka_value eleValue;
        int subCounter = 0;

        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;

        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);

        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of movc can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 6, INTERP_ERR_SYNTAX_FAILED, _("The position variable can only have 6 components!"));
            for (size_t i = 0; i < eleValue.arrValue.size(); ++i) block->movc_mid_point[i] = eleValue.arrValue[i];
        }
        else if (nArgIndex == 1)
        {
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The second argument of movc can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 6, INTERP_ERR_SYNTAX_FAILED, _("The position variable can only have 6 components!"));
            for (size_t i = 0; i < eleValue.arrValue.size(); ++i) block->movc_end_point[i] = eleValue.arrValue[i];
        }
        else if (nArgIndex == 2)
        {  // is relative motion or not
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The third argument of movc can be only 0 or 1!"));
            block->rel_motion_flag = (fabs(eleValue.scalarValue) >= 1e-6);  // zero --> false, non-zero --> true
        }
        else if (nArgIndex == 3)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The forth argument of movc can be only a scalar value!"));
            CHKS((eleValue.scalarValue < 0.0), INTERP_ERR_SYNTAX_FAILED, _("The velocity of movc cannot be a negative value!"));
            block->cmd_vel = eleValue.scalarValue;
        }
        else if (nArgIndex == 4)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The fifth argument of movc can be only a scalar value!"));
            CHKS((eleValue.scalarValue < 0.0), INTERP_ERR_SYNTAX_FAILED, _("The acceleration cannot be a negative value!"));
            block->acc = eleValue.scalarValue;
        }
        else if (nArgIndex == 5)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The sixth argument of movc can be only a scalar value!"));
            CHKS((eleValue.scalarValue < 0.0), INTERP_ERR_SYNTAX_FAILED, _("The tolerance cannot be a negative value!"));
            block->tolerance = eleValue.scalarValue;
        }
        else if (nArgIndex == 6)
        {
            // optional argument
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The seventh argument of movc can be only a scalar or array value!"));
            CHKS((eleValue.scalarValue < 0.0), INTERP_ERR_SYNTAX_FAILED, _("The count cannot be a negative value!"));
            block->count = eleValue.scalarValue;
            //CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The seventh argument of movc can be only a array value!"));
        }
        else if (nArgIndex == 7)
        {
            // optional argument
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The tenth argument of movc can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 3, INTERP_ERR_SYNTAX_FAILED, _("The condition variable can only have 3 components!"));
            block->condition.di_type = eleValue.arrValue[0];
            block->condition.di_index = eleValue.arrValue[1];
            block->condition.di_state = eleValue.arrValue[2];
        }
        else if (nArgIndex == 8)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The eighth argument of movc can be only a scalar value!"));
            CHKS((eleValue.scalarValue < 0.0), INTERP_ERR_SYNTAX_FAILED, _("The planner cannot be a negative value!"));
            block->planner_type = eleValue.scalarValue;
        }
        else if (nArgIndex == 9)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The ninth argument of movc can be only a scalar value!"));
            CHKS((eleValue.scalarValue < 0), INTERP_ERR_SYNTAX_FAILED, _("The movc mode cannot be a negative value!"));
            CHKS((eleValue.scalarValue > 3), INTERP_ERR_SYNTAX_FAILED, _("The movc mode cannot greater than 3!"));
            block->movcmode = eleValue.scalarValue;
        }
        ++nArgIndex;
        if (bracketClosed)
            break;
        arrEle = strtok(NULL, split);
    }

    int mtn_end_cond = (block->condition.di_type >= 0 && block->condition.di_index >= 0);
    if (block->last_mtn_end_cond)
    {
        block->last_mtn_end_cond = mtn_end_cond;
        SYNCH_PARSE_EXEC;
    }
    block->last_mtn_end_cond = mtn_end_cond;

    CHKS(nArgIndex != 9 && nArgIndex != 10, INTERP_ERR_SYNTAX_FAILED, _("movc function can only accept 9 or 10 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
    *counter = strlen(line);

    // update the block motion type to JK_MOVL
    block->g_modes[1] = JK_MOVC;
    if (endForceCondFlag == 1)
    {
        rtapi_print("endForceCondFlag------: %d, %s\n", endForceCondFlag, line);
        set_endforcecond_flag(2);
    }
    return INTERP_OK;
}

int Interp::read_movs(char* line, int* counter, block_pointer block, double* parameters)
{
    /*command format: movl(#<postTo>, rel/abs, vel)*/
    if (_setup.defining_sub)
        return INTERP_OK;

    // skip the space character, and check the '(' token
    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    //printf( "--------read_movS function------: counter =%d   c = %c  \n  ", *counter,line[*counter]);
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    //	End_Cond defCond = {-1, -1, 0};	// disabled by default
    //	block->condition = defCond;

    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char argsText[LINELEN];
    strcpy(argsText, line + *counter);
    char* arrEle = strtok(argsText, split);
    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        jaka_value eleValue;
        int subCounter = 0;

        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;

        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);

        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of movs can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 6, INTERP_ERR_SYNTAX_FAILED, _("The parameter variable can only have 6 components!"));
            for (size_t i = 0; i < eleValue.arrValue.size(); ++i) block->movs_a0[i] = eleValue.arrValue[i];
        }
        else if (nArgIndex == 1)
        {
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The second argument of movs can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 6, INTERP_ERR_SYNTAX_FAILED, _("The parameter variable can only have 6 components!"));
            for (size_t i = 0; i < eleValue.arrValue.size(); ++i) block->movs_a1[i] = eleValue.arrValue[i];
        }
        else if (nArgIndex == 2)
        {
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The third argument of movs can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 6, INTERP_ERR_SYNTAX_FAILED, _("The parameter variable can only have 6 components!"));
            for (size_t i = 0; i < eleValue.arrValue.size(); ++i) block->movs_a2[i] = eleValue.arrValue[i];
        }
        else if (nArgIndex == 3)
        {
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The forth argument of movs can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 6, INTERP_ERR_SYNTAX_FAILED, _("The parameter variable can only have 6 components!"));
            for (size_t i = 0; i < eleValue.arrValue.size(); ++i) block->movs_a3[i] = eleValue.arrValue[i];
        }
        else if (nArgIndex == 4)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The fifth argument of movs can be only a scalar value!"));
            CHKS((eleValue.scalarValue < 0.0), INTERP_ERR_SYNTAX_FAILED, _("The time cannot be a negative value!"));
            block->movs_time = eleValue.scalarValue;
        }

        ++nArgIndex;
        if (bracketClosed)
            break;
        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 5, INTERP_ERR_SYNTAX_FAILED, _("movs function can only accept 5 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
    *counter = strlen(line);

    // update the block motion type to JK_MOVS
    block->g_modes[1] = JK_MOVS;
    return INTERP_OK;
}

std::string Interp::read_execute_script_path(char* line)
{
    std::string tmp = line;
    int start = tmp.find('"');
    int end = tmp.find('"', start + 1);
    tmp = tmp.substr(start + 1, end - start - 1);
    // if(tmp[0]=='/')
    // 	tmp = ZUC_SCRIPT_DIR"" + tmp;
    // else
    // 	tmp = ZUC_SCRIPT_DIR"/" + tmp;

    return tmp;
}

int Interp::read_execute_script(char* line, int* counter, block_pointer block, setup_pointer settings)
{
    SYNCH_PARSE_EXEC;
    if (progStartLine > settings->sequence_number)
    {
        return INTERP_OK;
    }
    std::string filename = read_execute_script_path(line);
    SubFileMsg tmp;
    tmp.file_pointer = settings->file_pointer;
    tmp.lastLine = settings->lastLine;
    for (int i = 0; i < MAX_NESTED_REMAPS; ++i) { tmp.blocks[i] = settings->blocks[i]; }
    tmp.sequence_number = settings->sequence_number;
    tmp.filename = settings->filename;
    tmp.newFilename = filename;
    tmp.offsetMap = settings->offset_map;
    tmp.owordMap = m_owordIdMap;
    settings->sub_file_pointer.push_back(tmp);
    settings->offset_map.clear();
    m_owordIdMap.clear();

    try
    {
        for (int i = 0; i < (int)settings->sub_filename.size(); ++i)
        {
            if (settings->sub_filename[i] == filename)
            {
                return INTERP_LOOP_NESTING_ERROR;
            }
        }
        settings->sub_filename.push_back(filename);
        auto iter = _setup.sub_program_offset.find(filename);
        if (iter != _setup.sub_program_offset.end())
        {
            settings->offset_map = iter->second.offsetMap;
            m_owordIdMap = iter->second.owordMap;
        }

        strcpy(settings->filename, filename.c_str());
        settings->file_pointer = fopen(filename.c_str(), "r");
        settings->lastLine = 0;
        settings->sequence_number = 0;
        settings->current_line = 0;
        if (settings->file_pointer == NULL)
        {
            scripts_syncy_parse_execute(line, counter, block, settings);
            return INTERP_SUB_FILE_NOT_OPEN;
        }

        int ret = initOffsetMap(filename);
        // if (ret != INTERP_OK)
        // {
        //     return ret;
        // }
        return INTERP_OK;
    }
    catch (...)
    {
        printf("open execute script fail\n");
        return INTERP_FILE_NOT_OPEN;
    }
    return INTERP_SUB_FILE;
}

int Interp::scripts_syncy_parse_execute(char* line, int* counter, block_pointer block, setup_pointer settings)
{
    SYNCH_PARSE_EXEC;

    fclose(_setup.file_pointer);
    SubFileMsg raw_data, tmp;
    raw_data = _setup.sub_file_pointer.back();
    tmp.offsetMap = _setup.offset_map;
    tmp.owordMap = m_owordIdMap;
    _setup.sub_program_offset[raw_data.newFilename] = tmp;
    _setup.sub_file_pointer.pop_back();
    _setup.file_pointer = raw_data.file_pointer;
    _setup.lastLine = raw_data.lastLine;
    _setup.sequence_number = raw_data.sequence_number;
    _setup.offset_map = raw_data.offsetMap;
    m_owordIdMap = raw_data.owordMap;
    strcpy(_setup.filename, raw_data.filename.c_str());  //
    for (int i = 0; i < MAX_NESTED_REMAPS; ++i) { _setup.blocks[i] = raw_data.blocks[i]; }
    _setup.sub_filename.pop_back();
    return INTERP_OK;
}

int Interp::read_sleep(char* line, int* counter, block_pointer block, double* parameters)
{
    if (_setup.defining_sub)
        return INTERP_OK;

    SYNCH_PARSE_EXEC;

    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);

    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        int subCounter = 0;
        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;

        jaka_value eleValue;

        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);

        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of sleep can be only a scalar value!"));
            CHKS((eleValue.scalarValue < 0.0), INTERP_ERR_SYNTAX_FAILED, _("The time to sleep cannot be a negative value!"));
            block->sleep_time = eleValue.scalarValue;
        }

        ++nArgIndex;
        if (bracketClosed)
            break;

        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 1, INTERP_ERR_SYNTAX_FAILED, _("sleep function can only accept 1 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
    *counter = strlen(line);

    // update the block motion type to JK_MOVL
    block->g_modes[GM_MODAL_0] = JK_SLEEP;
    block->from_thread = m_threadId;
    _waitFlag = 1;
    return INTERP_OK;
}

int Interp::read_admitenable(char* line, int* counter, block_pointer block, double* parameters)
{
    if (_setup.defining_sub)
        return INTERP_OK;

    SYNCH_PARSE_EXEC;

    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);

    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        int subCounter = 0;
        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;

        jaka_value eleValue;

        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);

        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of sleep can be only a scalar value!"));
            CHKS((eleValue.scalarValue < 0.0), INTERP_ERR_SYNTAX_FAILED, _("The time to sleep cannot be a negative value!"));
            block->admit_enable = round(eleValue.scalarValue);
        }

        ++nArgIndex;
        if (bracketClosed)
            break;

        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 1, INTERP_ERR_SYNTAX_FAILED, _("sleep function can only accept 1 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
    *counter = strlen(line);

    block->g_modes[GM_MODAL_0] = JK_ENA_ADMIT;
    return INTERP_OK;
}

int Interp::read_setdout(char* line, int* counter, block_pointer block, double* parameters)
{
    /*command format: dout(index, tarState, bSynchronized)*/
    if (_setup.defining_sub)
        return INTERP_OK;

    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);

    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        jaka_value eleValue;
        int subCounter = 0;

        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;

        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);

        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of setdout can be only an integer!"));
            block->ioType = static_cast<int>(fabs(eleValue.scalarValue));
        }
        else if (nArgIndex == 1)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of setdout can be only an integer!"));
            block->ioIndex = static_cast<int>(fabs(eleValue.scalarValue));
        }
        else if (nArgIndex == 2)
        {  // targert output status
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The second argument of setdout can be only 0 or 1!"));
            block->dout_tar_state = (fabs(eleValue.scalarValue) >= 1e-6);  // zero --> false, non-zero --> true
        }
        else if (nArgIndex == 3)
        {  // immediate flag
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The Third argument of setdout can be only 0 or 1!"));
            block->out_imme_flag = (fabs(eleValue.scalarValue) >= 1e-6);  // zero --> false, non-zero --> true
            if (m_threadId)                                               //  子线程中将运动中设置DO 改为 即时设置DO
            {
                block->out_imme_flag = 1;
            }
            if (block->out_imme_flag)
            {
                // rtapi_print("SYNCH test before>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
                SYNCH_PARSE_EXEC;
                // rtapi_print("SYNCH test after<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<\n");
            }
        }

        ++nArgIndex;
        if (bracketClosed)
            break;

        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 4, INTERP_ERR_SYNTAX_FAILED, _("dout function can only accept 3 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
    *counter = strlen(line);

    // update the block motion type to JK_MOVL
    block->m_modes[5] = JK_DOUT;
    block->from_thread = m_threadId;
    return INTERP_OK;
}

int Interp::read_setaout(char* line, int* counter, block_pointer block, double* parameters)
{
    /*command format: dout(index, tarState, bSynchronized)*/
    if (_setup.defining_sub)
        return INTERP_OK;

    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);

    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        jaka_value eleValue;
        int subCounter = 0;

        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;

        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);

        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of setaout can be only an integer!"));
            block->ioType = static_cast<int>(fabs(eleValue.scalarValue));
        }
        else if (nArgIndex == 1)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of setaout can be only an integer!"));
            block->ioIndex = static_cast<int>(fabs(eleValue.scalarValue));
        }
        else if (nArgIndex == 2)
        {  // targert output status
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The second argument of setaout can be only 0 or 1!"));
            block->aout_tar_value = eleValue.scalarValue;  // zero --> false, non-zero --> true
        }
        else if (nArgIndex == 3)
        {  // immediate flag
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The Third argument of setaout can be only 0 or 1!"));
            block->out_imme_flag = (fabs(eleValue.scalarValue) >= 1e-6);  // zero --> false, non-zero --> true
            if (m_threadId)                                               //  子线程中将运动中设置AO 改为 即时设置AO
            {
                block->out_imme_flag = 1;
            }
            if (block->out_imme_flag)
            {
                // rtapi_print("SYNCH test before>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
                SYNCH_PARSE_EXEC;
                // rtapi_print("SYNCH test after<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<\n");
            }
        }

        ++nArgIndex;
        if (bracketClosed)
            break;

        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 4, INTERP_ERR_SYNTAX_FAILED, _("aout function can only accept 3 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));
    *counter = strlen(line);

    // update the block motion type to JK_MOVL
    block->m_modes[5] = JK_AOUT;

    return INTERP_OK;
}

int Interp::read_kinInverse(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    if (value_ptr == nullptr)
    {
        *counter = strlen(line);
        return INTERP_OK;
    }
    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);

    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    jaka_value resValue;
    resValue.valueType = ARRAY_VALUE;
    double joint[6];
    for (size_t i = 0; i < 6; i++) resValue.arrValue.push_back(0);

    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        int subCounter = 0;
        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;

        jaka_value eleValue;
        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);
        if (0 == nArgIndex)
        {
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of poseadd can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 6, INTERP_ERR_SYNTAX_FAILED, _("The joint variable can only have 6 components!"));
            //for (size_t i = 0; i < 6; ++i)
            //	resValue.arrValue[i] += eleValue.arrValue[i];
            //printf("[ read_kinInverse ] : joint = %f  %f  %f   %f   %f   %f   \n",
            //eleValue.arrValue[0],eleValue.arrValue[1],eleValue.arrValue[2],eleValue.arrValue[3],eleValue.arrValue[4],eleValue.arrValue[5]
            //);
            for (int i = 0; i < 6; ++i) { joint[i] = eleValue.arrValue[i]; }
        }
        if (1 == nArgIndex)
        {
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The second argument of poseadd can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 6, INTERP_ERR_SYNTAX_FAILED, _("The position variable can only have 6 components!"));
            //for (size_t i = 0; i < 6; ++i)
            //	resValue.arrValue[i] += eleValue.arrValue[i];
            //printf("[ read_kinInverse ] : pos = %f  %f  %f   %f   %f   %f   \n",
            //eleValue.arrValue[0],eleValue.arrValue[1],eleValue.arrValue[2],eleValue.arrValue[3],eleValue.arrValue[4],eleValue.arrValue[5]
            //);
            ZucPose world = {0}, world_temp = {0};
            world.tran.x = eleValue.arrValue[0];
            world.tran.y = eleValue.arrValue[1];
            world.tran.z = eleValue.arrValue[2];
            world.a = eleValue.arrValue[3];
            world.b = eleValue.arrValue[4];
            world.c = eleValue.arrValue[5];
            //printf(" [ read_kinInverse ] : world = %f %f %f %f %f %f \n",world.tran.x,world.tran.y,world.tran.z,world.a,world.b,world.c);
            unsigned long fflags = 0, iflags = 0;
            GET_KINEMATICS_FORWARD(joint, &world_temp, &fflags, &iflags, &_setup.tool_offset, &_setup.user_offset);
            if (!GET_KINEMATICS_INVERSE(&world, joint, &iflags, &fflags, &_setup.tool_offset, &_setup.user_offset))
            {
                for (int i = 0; i < 6; ++i) { resValue.arrValue[i] = joint[i]; }
            }
            else
            {
                ERS(INTERP_ERR_INVERSE_FAILED, NCE_INTERP_KIN_INVERSE_FAILED);
            }
        }

        ++nArgIndex;
        if (bracketClosed)
            break;

        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 2, INTERP_ERR_SYNTAX_FAILED, _("kinInverse function can only accept 2 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));

    /* save the result of pos addition*/
    value_ptr->valueType = ARRAY_VALUE;
    value_ptr->arrValue = resValue.arrValue;
    *counter = strlen(line);
    return INTERP_OK;
}

int Interp::read_kinForward(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    if (value_ptr == nullptr)
    {
        *counter = strlen(line);
        return INTERP_OK;
    }
    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);

    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    jaka_value resValue;
    resValue.valueType = ARRAY_VALUE;
    double joint[6];
    ZucPose world = {0};

    for (size_t i = 0; i < 6; i++) resValue.arrValue.push_back(0);

    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        int subCounter = 0;
        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;

        jaka_value eleValue;
        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);
        if (0 == nArgIndex)
        {
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of poseadd can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 6, INTERP_ERR_SYNTAX_FAILED, _("The joint variable can only have 6 components!"));
            //for (size_t i = 0; i < 6; ++i)
            //	resValue.arrValue[i] += eleValue.arrValue[i];
            //printf("[ read_kinInverse ] : joint = %f  %f  %f   %f   %f   %f   \n",
            //eleValue.arrValue[0],eleValue.arrValue[1],eleValue.arrValue[2],eleValue.arrValue[3],eleValue.arrValue[4],eleValue.arrValue[5]
            //);
            for (int i = 0; i < 6; ++i) { joint[i] = eleValue.arrValue[i]; }

            unsigned long fflags = 0, iflags = 0;
            if (!GET_KINEMATICS_FORWARD(joint, &world, &fflags, &iflags, &_setup.tool_offset, &_setup.user_offset))
            {
                resValue.arrValue[0] = world.tran.x;
                resValue.arrValue[1] = world.tran.y;
                resValue.arrValue[2] = world.tran.z;
                resValue.arrValue[3] = world.a;
                resValue.arrValue[4] = world.b;
                resValue.arrValue[5] = world.c;
            }
            else
            {
                ERS(INTERP_ERR_INVERSE_FAILED, NCE_INTERP_KIN_FORWARD_FAILED);
            }
        }
        ++nArgIndex;
        if (bracketClosed)
            break;

        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 1, INTERP_ERR_SYNTAX_FAILED, _("kinForward function can only accept 1 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));

    /* save the result of pos addition*/
    value_ptr->valueType = ARRAY_VALUE;
    value_ptr->arrValue = resValue.arrValue;
    *counter = strlen(line);
    return INTERP_OK;
}

int Interp::read_isPoseReachable(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    // if there is a cartesian trajecotry to reach the target pose based on crrent pose
    if (_setup.defining_sub)
        return INTERP_OK;

    SYNCH_PARSE_EXEC;

    if (value_ptr == nullptr)
    {
        *counter = strlen(line);
        return INTERP_OK;
    }

    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);

    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    jaka_value resValue;
    resValue.valueType = ARRAY_VALUE;
    for (size_t i = 0; i < 6; i++) resValue.arrValue.push_back(0);

    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        int subCounter = 0;
        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;

        jaka_value eleValue;
        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);
        if (0 == nArgIndex)
        {
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The second argument of poseadd can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 6, INTERP_ERR_SYNTAX_FAILED, _("The position variable can only have 6 components!"));
            // get current joint position
            double jointPos[6] = {0};
            ZucPose world = {0}, world_temp = {0};
            unsigned long fflags = 0, iflags = 0;
            usrmotGetAtlJointPose(jointPos);
            world.tran.x = eleValue.arrValue[0];
            world.tran.y = eleValue.arrValue[1];
            world.tran.z = eleValue.arrValue[2];
            world.a = eleValue.arrValue[3];
            world.b = eleValue.arrValue[4];
            world.c = eleValue.arrValue[5];
            GET_KINEMATICS_FORWARD(jointPos, &world_temp, &fflags, &iflags, &_setup.tool_offset, &_setup.user_offset);
            if (!GET_KINEMATICS_INVERSE(&world, jointPos, &iflags, &fflags, &_setup.tool_offset, &_setup.user_offset))
            {
                resValue.scalarValue = 1;
                rtapi_print("arrivable, target jpos: %f, %f, %f, %f, %f, %f\n", jointPos[0], jointPos[1], jointPos[2], jointPos[3], jointPos[4], jointPos[5]);
            }
            else
            {
                resValue.scalarValue = 0;
            }
        }

        ++nArgIndex;
        if (bracketClosed)
            break;
        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 1, INTERP_ERR_SYNTAX_FAILED, _("is_pose_reachable function can only accept 1 argument!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));

    value_ptr->valueType = SCALAR_VALUE;
    value_ptr->scalarValue = resValue.scalarValue;
    *counter = strlen(line);
    return INTERP_OK;
}

int Interp::read_poseadd(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);

    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    jaka_value resValue, deltValue;
    resValue.valueType = ARRAY_VALUE;
    for (size_t i = 0; i < 6; i++) resValue.arrValue.push_back(0);

    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        int subCounter = 0;
        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;

        jaka_value eleValue;
        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);

        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of poseadd can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 6, INTERP_ERR_SYNTAX_FAILED, _("The position variable can only have 6 components!"));
            resValue = eleValue;
        }
        else if (nArgIndex == 1)
        {
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of poseadd can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 6, INTERP_ERR_SYNTAX_FAILED, _("The position variable can only have 6 components!"));
            deltValue = eleValue;
        }
        ++nArgIndex;
        if (bracketClosed)
            break;

        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 2, INTERP_ERR_SYNTAX_FAILED, _("poseadd function can only accept 2 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));

    for (size_t i = 0; i < 3; ++i)
    {  // position add
        resValue.arrValue[i] += deltValue.arrValue[i];
    }
    PmRpy iniRpy, deltRpy, resRpy;  // orientation mult
    iniRpy.y = resValue.arrValue[3] * PM_PI / 180.0;
    iniRpy.p = resValue.arrValue[4] * PM_PI / 180.0;
    iniRpy.r = resValue.arrValue[5] * PM_PI / 180.0;
    deltRpy.y = deltValue.arrValue[3] * PM_PI / 180.0;
    deltRpy.p = deltValue.arrValue[4] * PM_PI / 180.0;
    deltRpy.r = deltValue.arrValue[5] * PM_PI / 180.0;
    pmRpyRpyAdd(&iniRpy, &deltRpy, &resRpy);
    resValue.arrValue[3] = resRpy.y * 180.0 / PM_PI;
    resValue.arrValue[4] = resRpy.p * 180.0 / PM_PI;
    resValue.arrValue[5] = resRpy.r * 180.0 / PM_PI;

    /* save the result of pos addition*/
    value_ptr->valueType = ARRAY_VALUE;
    value_ptr->arrValue = resValue.arrValue;
    *counter = strlen(line);
    return INTERP_OK;
}

int Interp::read_posesub(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);

    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    jaka_value resValue, deltValue;
    resValue.valueType = ARRAY_VALUE;
    for (size_t i = 0; i < 6; i++) resValue.arrValue.push_back(0);

    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        int subCounter = 0;
        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;

        jaka_value eleValue;
        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);

        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of poseadd can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 6, INTERP_ERR_SYNTAX_FAILED, _("The position variable can only have 6 components!"));
            resValue = eleValue;
        }
        else if (nArgIndex == 1)
        {
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of poseadd can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 6, INTERP_ERR_SYNTAX_FAILED, _("The position variable can only have 6 components!"));
            deltValue = eleValue;
        }
        ++nArgIndex;
        if (bracketClosed)
            break;

        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 2, INTERP_ERR_SYNTAX_FAILED, _("poseadd function can only accept 2 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));

    for (size_t i = 0; i < 3; ++i)
    {  // position add
        resValue.arrValue[i] -= deltValue.arrValue[i];
    }
    PmRpy iniRpy, deltRpy, resRpy;  // orientation mult
    iniRpy.y = resValue.arrValue[3] * PM_PI / 180.0;
    iniRpy.p = resValue.arrValue[4] * PM_PI / 180.0;
    iniRpy.r = resValue.arrValue[5] * PM_PI / 180.0;
    deltRpy.y = deltValue.arrValue[3] * PM_PI / 180.0;
    deltRpy.p = deltValue.arrValue[4] * PM_PI / 180.0;
    deltRpy.r = deltValue.arrValue[5] * PM_PI / 180.0;
    pmRpyRpySub(&iniRpy, &deltRpy, &resRpy);
    resValue.arrValue[3] = resRpy.y * 180.0 / PM_PI;
    resValue.arrValue[4] = resRpy.p * 180.0 / PM_PI;
    resValue.arrValue[5] = resRpy.r * 180.0 / PM_PI;

    /* save the result of pos addition*/
    value_ptr->valueType = ARRAY_VALUE;
    value_ptr->arrValue = resValue.arrValue;
    *counter = strlen(line);
    return INTERP_OK;
}

int Interp::read_poseinv(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    if (value_ptr == nullptr)
    {
        *counter = strlen(line);
        return INTERP_OK;
    }
    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);

    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    jaka_value resValue;
    resValue.valueType = ARRAY_VALUE;
    for (size_t i = 0; i < 6; i++) resValue.arrValue.push_back(0);

    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        int subCounter = 0;
        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;

        jaka_value eleValue;
        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);

        if (nArgIndex < 1)
        {
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The argument of poseinv can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 6, INTERP_ERR_SYNTAX_FAILED, _("The position variable can only have 6 components!"));
            for (size_t i = 0; i < 6; ++i) resValue.arrValue[i] = eleValue.arrValue[i];
        }
        ++nArgIndex;
        if (bracketClosed)
            break;

        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 1, INTERP_ERR_SYNTAX_FAILED, _("poseinv function can only accept 1 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));

    PmPose pos_origin;
    PmRpy rpy_origin;
    pos_origin.tran.x = resValue.arrValue[0];
    pos_origin.tran.y = resValue.arrValue[1];
    pos_origin.tran.z = resValue.arrValue[2];
    rpy_origin.y = resValue.arrValue[3] * PM_PI / 180.0;
    rpy_origin.p = resValue.arrValue[4] * PM_PI / 180.0;
    rpy_origin.r = resValue.arrValue[5] * PM_PI / 180.0;
    pmRpyQuatConvert(&rpy_origin, &pos_origin.rot);
    // rtapi_print("inv initPos: %f, %f, %f, %f, %f, %f\n", resValue.arrValue[0], resValue.arrValue[1], resValue.arrValue[2], resValue.arrValue[3], resValue.arrValue[4], resValue.arrValue[5]);
    // rtapi_print("inv pos_origin: %f, %f, %f, %f, %f, %f, %f\n", pos_origin.tran.x, pos_origin.tran.y, pos_origin.tran.z, pos_origin.rot.s, pos_origin.rot.x, pos_origin.rot.y, pos_origin.rot.z);

    PmPose pos_res;
    PmRpy rpy_res;
    pmPoseInv(&pos_origin, &pos_res);
    pmQuatRpyConvert(&pos_res.rot, &rpy_res);

    resValue.arrValue[0] = pos_res.tran.x;
    resValue.arrValue[1] = pos_res.tran.y;
    resValue.arrValue[2] = pos_res.tran.z;
    resValue.arrValue[3] = rpy_res.y * 180.0 / PM_PI;
    resValue.arrValue[4] = rpy_res.p * 180.0 / PM_PI;
    resValue.arrValue[5] = rpy_res.r * 180.0 / PM_PI;

    value_ptr->valueType = ARRAY_VALUE;
    value_ptr->arrValue = resValue.arrValue;

    // rtapi_print("The inverse of the position is: %f, %f, %f, %f, %f, %f\n",
    //   value_ptr->arrValue[0], value_ptr->arrValue[1], value_ptr->arrValue[2], value_ptr->arrValue[3], value_ptr->arrValue[4], value_ptr->arrValue[5]);

    *counter = strlen(line);
    return INTERP_OK;
}

int Interp::read_posedist(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    if (value_ptr == nullptr)
    {
        *counter = strlen(line);
        return INTERP_OK;
    }
    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);

    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    jaka_value pos1, pos2;

    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        int subCounter = 0;
        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;

        jaka_value eleValue;
        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);

        if (nArgIndex == 0)
        {
            pos1 = eleValue;
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The argument of posedist can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 6, INTERP_ERR_SYNTAX_FAILED, _("The position variable can only have 6 components!"));
        }
        else if (nArgIndex == 1)
        {
            pos2 = eleValue;
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The argument of posedist can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 6, INTERP_ERR_SYNTAX_FAILED, _("The position variable can only have 6 components!"));
        }

        ++nArgIndex;
        if (bracketClosed)
            break;

        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 2, INTERP_ERR_SYNTAX_FAILED, _("posetrans function can only accept 2 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));

    double vec_dist[3] = {0.0, 0.0, 0.0};
    for (size_t i = 0; i < 3; i++) vec_dist[i] = pos2.arrValue[i] - pos1.arrValue[i];

    value_ptr->valueType = SCALAR_VALUE;
    value_ptr->scalarValue = sqrtf(vec_dist[0] * vec_dist[0] + vec_dist[1] * vec_dist[1] + vec_dist[2] * vec_dist[2]);

    // rtapi_print("The distance between the two points (%f, %f, %f), (%f, %f, %f) is: %f\n",
    //             pos1.arrValue[0], pos1.arrValue[1], pos1.arrValue[2], pos2.arrValue[0], pos2.arrValue[1], pos2.arrValue[2], value_ptr->scalarValue);

    *counter = strlen(line);

    return INTERP_OK;
}

int Interp::read_posetrans(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    if (value_ptr == nullptr)
    {
        *counter = strlen(line);
        return INTERP_OK;
    }
    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);

    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    jaka_value pos1, pos2;

    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        int subCounter = 0;
        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;

        jaka_value eleValue;
        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);

        if (nArgIndex == 0)
        {
            pos1 = eleValue;
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The argument of posetrans can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 6, INTERP_ERR_SYNTAX_FAILED, _("The position variable can only have 6 components!"));
        }
        else if (nArgIndex == 1)
        {
            pos2 = eleValue;
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The argument of posetrans can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 6, INTERP_ERR_SYNTAX_FAILED, _("The position variable can only have 6 components!"));
        }

        ++nArgIndex;
        if (bracketClosed)
            break;

        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 2, INTERP_ERR_SYNTAX_FAILED, _("posetrans function can only accept 2 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));

    PmPose pos_base;
    PmRpy rpy_base;
    pos_base.tran.x = pos1.arrValue[0];
    pos_base.tran.y = pos1.arrValue[1];
    pos_base.tran.z = pos1.arrValue[2];
    rpy_base.y = pos1.arrValue[3] * PM_PI / 180.0;
    rpy_base.p = pos1.arrValue[4] * PM_PI / 180.0;
    rpy_base.r = pos1.arrValue[5] * PM_PI / 180.0;
    pmRpyQuatConvert(&rpy_base, &pos_base.rot);

    PmPose pos_trans;
    PmRpy rpy_trans;
    pos_trans.tran.x = pos2.arrValue[0];
    pos_trans.tran.y = pos2.arrValue[1];
    pos_trans.tran.z = pos2.arrValue[2];
    rpy_trans.y = pos2.arrValue[3] * PM_PI / 180.0;
    rpy_trans.p = pos2.arrValue[4] * PM_PI / 180.0;
    rpy_trans.r = pos2.arrValue[5] * PM_PI / 180.0;
    pmRpyQuatConvert(&rpy_trans, &pos_trans.rot);

    PmPose pos_res;
    PmRpy rpy_res;
    pmPosePoseMult(&pos_base, &pos_trans, &pos_res);

    pmQuatRpyConvert(&pos_res.rot, &rpy_res);

    *value_ptr = pos1;
    value_ptr->arrValue[0] = pos_res.tran.x;
    value_ptr->arrValue[1] = pos_res.tran.y;
    value_ptr->arrValue[2] = pos_res.tran.z;
    value_ptr->arrValue[3] = rpy_res.y * 180.0 / PM_PI;
    value_ptr->arrValue[4] = rpy_res.p * 180.0 / PM_PI;
    value_ptr->arrValue[5] = rpy_res.r * 180.0 / PM_PI;

    // rtapi_print("The result of postrans is: %f, %f, %f, %f, %f, %f\n",
    //   value_ptr->arrValue[0], value_ptr->arrValue[1], value_ptr->arrValue[2], value_ptr->arrValue[3], value_ptr->arrValue[4], value_ptr->arrValue[5]);

    *counter = strlen(line);

    return INTERP_OK;
}

int Interp::read_poseintpl(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    if (value_ptr == nullptr)
    {
        *counter = strlen(line);
        return INTERP_OK;
    }
    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);

    int ret;
    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    double alpha = 0.0;
    jaka_value pos1, pos2;
    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        int subCounter = 0;
        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;

        jaka_value eleValue;
        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);

        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The first argument of poseintpl can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 6, INTERP_ERR_SYNTAX_FAILED, _("The position variable can only have 6 components!"));
            pos1 = eleValue;
        }
        else if (nArgIndex == 1)
        {
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The second argument of poseintpl can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 6, INTERP_ERR_SYNTAX_FAILED, _("The position variable can only have 6 components!"));
            pos2 = eleValue;
        }
        else if (nArgIndex == 2)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The third argument of poseintpl can be only an scalar variable!"));
            alpha = eleValue.scalarValue;
            if (fabs(alpha) <= 0.00001)
            {
                alpha = 0.0;
            }
            else if (fabs(alpha - 1) <= 0.00001)
            {
                alpha = 1.0;
            }
            else if (alpha < 0.0 || alpha > 1.0)
            {
                // report error, then return
                ERS(INTERP_ERR_INVALID_INTPL_RATIO, _("invalid coefficient for pose interpolation, should be in [0, 1]!"));
            }
        }

        ++nArgIndex;
        if (bracketClosed)
            break;

        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 3, INTERP_ERR_SYNTAX_FAILED, _("poseintpl function can only accept 3 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));

    /* save the result of pos addition*/
    value_ptr->valueType = ARRAY_VALUE;
    value_ptr->arrValue = pos1.arrValue;

    if (!parsing_flag)
    {
        /*interpolation via quaternion*/
        // PmRpy rpy1 = {pos1.arrValue[3]*PM_PI/180.0, pos1.arrValue[4]*PM_PI/180.0, pos1.arrValue[5]*PM_PI/180.0};
        // PmRpy rpy2 = {pos2.arrValue[3]*PM_PI/180.0, pos2.arrValue[4]*PM_PI/180.0, pos2.arrValue[5]*PM_PI/180.0};
        // PmRpy deltRpy, resRpy;
        // PmRotationMatrix oldOMtrix, newOMtrix, deltOMtrix, invOldOMtrix;
        // pmRpyMatConvert(&rpy1, &oldOMtrix);
        // pmRpyMatConvert(&rpy2, &newOMtrix);
        // pmMatInv(&oldOMtrix, &invOldOMtrix);
        // pmMatMatMult(&newOMtrix, &invOldOMtrix, &deltOMtrix);
        // pmMatRpyConvert(&deltOMtrix, &deltRpy);	// deltRpy = Rpy2*Inv(Rpy1)
        // deltRpy.y = deltRpy.y * alpha;
        // deltRpy.p = deltRpy.p * alpha;
        // deltRpy.r = deltRpy.r * alpha;
        // pmRpyRpyAdd(&rpy1, &deltRpy, &resRpy);  // deltRpy*Rpy1 = resRPY
        // value_ptr->arrValue[3] = resRpy.y*180.0/PM_PI;
        // value_ptr->arrValue[4] = resRpy.p*180.0/PM_PI;
        // value_ptr->arrValue[5] = resRpy.r*180.0/PM_PI;
        // for(size_t i=0; i<3; i++){
        //   value_ptr->arrValue[i] = pos1.arrValue[i] + (pos2.arrValue[i] - pos1.arrValue[i]) * alpha;
        // }
        /*interpolation via rpy directly*/
        for (size_t i = 0; i < 6; i++)
        {
            if (i >= 3)
            {
                if (pos2.arrValue[i] - pos1.arrValue[i] > 180.0)
                    pos2.arrValue[i] -= 360.0;
                else if (pos2.arrValue[i] - pos1.arrValue[i] < -180.0)
                    pos2.arrValue[i] += 360.0;
            }
            value_ptr->arrValue[i] = pos1.arrValue[i] + (pos2.arrValue[i] - pos1.arrValue[i]) * alpha;
        }
        // rtapi_print("pos1: %f, %f, %f, %f, %f, %f\n", pos1.arrValue[0], pos1.arrValue[1], pos1.arrValue[2], pos1.arrValue[3], pos1.arrValue[4],
        // 		pos1.arrValue[5]);
        // rtapi_print("pos2: %f, %f, %f, %f, %f, %f\n", pos2.arrValue[0], pos2.arrValue[1], pos2.arrValue[2], pos2.arrValue[3], pos2.arrValue[4],
        // 		pos2.arrValue[5]);
        // rtapi_print("after: %f, %f, %f, %f, %f, %f\n", value_ptr->arrValue[0], value_ptr->arrValue[1], value_ptr->arrValue[2], value_ptr->arrValue[3],
        // 		value_ptr->arrValue[4], value_ptr->arrValue[5]);
    }

    *counter = strlen(line);
    return INTERP_OK;
}

int Interp::read_xyplanetrans(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    int ret;
    if (value_ptr == nullptr)
    {
        *counter = strlen(line);
        return INTERP_OK;
    }
    value_ptr->valueType = ARRAY_VALUE;
    value_ptr->arrValue.resize(6);

    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);

    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    jaka_value basepos;
    double delt_x = 0.0, delt_y = 0.0, delt_rz = 0.0;
    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        int subCounter = 0;
        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;

        jaka_value eleValue;
        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);

        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The 1st argument of xyplanetrans can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 6, INTERP_ERR_SYNTAX_FAILED, _("The position variable can only have 6 components!"));
            basepos = eleValue;
        }
        else if (nArgIndex == 1)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The 2nd argument of xyplanetrans can be only an array variable!"));
            delt_x = eleValue.scalarValue;
        }
        else if (nArgIndex == 2)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The 3rd argument of xyplanetrans can be only an array variable!"));
            delt_y = eleValue.scalarValue;
        }
        else if (nArgIndex == 3)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The 4th argument of xyplanetrans can be only an array variable!"));
            delt_rz = eleValue.scalarValue;
        }

        ++nArgIndex;
        if (bracketClosed)
            break;

        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 4, INTERP_ERR_SYNTAX_FAILED, _("xyplanetrans function can only accept 4 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));

    PmPose pose_tar;
    PmRpy rpy_tar;
    pose_tar.tran.x = basepos.arrValue[0];
    pose_tar.tran.y = basepos.arrValue[1];
    pose_tar.tran.z = basepos.arrValue[2];
    rpy_tar.y = basepos.arrValue[3] * PM_PI / 180.0;
    rpy_tar.p = basepos.arrValue[4] * PM_PI / 180.0;
    rpy_tar.r = basepos.arrValue[5] * PM_PI / 180.0;
    pmRpyQuatConvert(&rpy_tar, &pose_tar.rot);

    PmPose pose_base = pose_tar;
    PmPose pose_delt;
    PmHomogeneous hom_delt;
    PmCartesian rotDir = {0, 0, 1};
    pmRotArbitAxisWithAngleConvert(&(pose_base.tran), &rotDir, delt_rz * PM_PI / 180.0, &hom_delt);
    pmHomPoseConvert(&hom_delt, &pose_delt);
    pmPosePoseMult(&pose_delt, &pose_base, &pose_tar);
    pmQuatRpyConvert(&pose_tar.rot, &rpy_tar);
    pose_tar.tran.x += delt_x;
    pose_tar.tran.y += delt_y;
    pose_tar.tran.z += 0;

    value_ptr->arrValue[0] = pose_tar.tran.x;
    value_ptr->arrValue[1] = pose_tar.tran.y;
    value_ptr->arrValue[2] = pose_tar.tran.z;
    value_ptr->arrValue[3] = rpy_tar.y * 180.0 / PM_PI;
    value_ptr->arrValue[4] = rpy_tar.p * 180.0 / PM_PI;
    value_ptr->arrValue[5] = rpy_tar.r * 180.0 / PM_PI;

    // rtapi_print("The result of postrans is: %f, %f, %f, %f, %f, %f\n",
    //   value_ptr->arrValue[0], value_ptr->arrValue[1], value_ptr->arrValue[2], value_ptr->arrValue[3], value_ptr->arrValue[4], value_ptr->arrValue[5]);

    *counter = strlen(line);
    return INTERP_OK;
}

int Interp::read_yzplanetrans(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    int ret;
    if (value_ptr == nullptr)
    {
        *counter = strlen(line);
        return INTERP_OK;
    }
    value_ptr->valueType = ARRAY_VALUE;
    value_ptr->arrValue.resize(6);

    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);

    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    jaka_value basepos;
    double delt_y = 0.0, delt_z = 0.0, delt_rx = 0.0;
    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        int subCounter = 0;
        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;

        jaka_value eleValue;
        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);

        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The 1st argument of yzplanetrans can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 6, INTERP_ERR_SYNTAX_FAILED, _("The position variable can only have 6 components!"));
            basepos = eleValue;
        }
        else if (nArgIndex == 1)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The 2nd argument of yzplanetrans can be only an array variable!"));
            delt_y = eleValue.scalarValue;
        }
        else if (nArgIndex == 2)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The 3rd argument of yzplanetrans can be only an array variable!"));
            delt_z = eleValue.scalarValue;
        }
        else if (nArgIndex == 3)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The 4th argument of yzplanetrans can be only an array variable!"));
            delt_rx = eleValue.scalarValue;
        }

        ++nArgIndex;
        if (bracketClosed)
            break;

        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 4, INTERP_ERR_SYNTAX_FAILED, _("yzplanetrans function can only accept 4 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));

    PmPose pose_tar;
    PmRpy rpy_tar;
    pose_tar.tran.x = basepos.arrValue[0];
    pose_tar.tran.y = basepos.arrValue[1];
    pose_tar.tran.z = basepos.arrValue[2];
    rpy_tar.y = basepos.arrValue[3] * PM_PI / 180.0;
    rpy_tar.p = basepos.arrValue[4] * PM_PI / 180.0;
    rpy_tar.r = basepos.arrValue[5] * PM_PI / 180.0;
    pmRpyQuatConvert(&rpy_tar, &pose_tar.rot);

    PmPose pose_base = pose_tar;
    PmPose pose_delt;
    PmHomogeneous hom_delt;
    PmCartesian rotDir = {1, 0, 0};
    pmRotArbitAxisWithAngleConvert(&(pose_base.tran), &rotDir, delt_rx * PM_PI / 180.0, &hom_delt);
    pmHomPoseConvert(&hom_delt, &pose_delt);
    pmPosePoseMult(&pose_delt, &pose_base, &pose_tar);
    pmQuatRpyConvert(&pose_tar.rot, &rpy_tar);
    pose_tar.tran.x += 0;
    pose_tar.tran.y += delt_y;
    pose_tar.tran.z += delt_z;

    value_ptr->arrValue[0] = pose_tar.tran.x;
    value_ptr->arrValue[1] = pose_tar.tran.y;
    value_ptr->arrValue[2] = pose_tar.tran.z;
    value_ptr->arrValue[3] = rpy_tar.y * 180.0 / PM_PI;
    value_ptr->arrValue[4] = rpy_tar.p * 180.0 / PM_PI;
    value_ptr->arrValue[5] = rpy_tar.r * 180.0 / PM_PI;

    // rtapi_print("The result of postrans is: %f, %f, %f, %f, %f, %f\n",
    // 	value_ptr->arrValue[0], value_ptr->arrValue[1], value_ptr->arrValue[2], value_ptr->arrValue[3], value_ptr->arrValue[4], value_ptr->arrValue[5]);

    *counter = strlen(line);
    return INTERP_OK;
}

int Interp::read_zxplanetrans(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    int ret;
    if (value_ptr == nullptr)
    {
        *counter = strlen(line);
        return INTERP_OK;
    }
    value_ptr->valueType = ARRAY_VALUE;
    value_ptr->arrValue.resize(6);
    while (*counter < (int)strlen(line) && line[*counter] == ' ') *counter = *counter + 1;
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    char argsText[LINELEN];
    strcpy(argsText, line + *counter);

    int nArgIndex = 0;
    bool bracketClosed = false;
    const char* split = ",";
    char* arrEle = strtok(argsText, split);
    jaka_value basepos;
    double delt_z = 0.0, delt_x = 0.0, delt_ry = 0.0;
    while (arrEle != NULL)
    {
        char* pchar = strstr(arrEle, ")");
        if (pchar != NULL)
        {  // contain the ')'
            bracketClosed = true;
            *pchar = '\0';
        }

        int subCounter = 0;
        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;

        jaka_value eleValue;
        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);

        if (nArgIndex == 0)
        {
            CHKS(eleValue.valueType != ARRAY_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The 1st argument of zxplanetrans can be only an array variable!"));
            CHKS(eleValue.arrValue.size() != 6, INTERP_ERR_SYNTAX_FAILED, _("The position variable can only have 6 components!"));
            basepos = eleValue;
        }
        else if (nArgIndex == 1)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The 2nd argument of zxplanetrans can be only an array variable!"));
            delt_z = eleValue.scalarValue;
        }
        else if (nArgIndex == 2)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The 3rd argument of zxplanetrans can be only an array variable!"));
            delt_x = eleValue.scalarValue;
        }
        else if (nArgIndex == 3)
        {
            CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("The 4th argument of zxplanetrans can be only an array variable!"));
            delt_ry = eleValue.scalarValue;
        }

        ++nArgIndex;
        if (bracketClosed)
            break;

        arrEle = strtok(NULL, split);
    }

    CHKS(nArgIndex != 4, INTERP_ERR_SYNTAX_FAILED, _("zxplanetrans function can only accept 4 arguments!"));
    CHKS(!bracketClosed, INTERP_ERR_SYNTAX_FAILED, _(") is required to close the function call!"));

    PmPose pose_tar;
    PmRpy rpy_tar;
    pose_tar.tran.x = basepos.arrValue[0];
    pose_tar.tran.y = basepos.arrValue[1];
    pose_tar.tran.z = basepos.arrValue[2];
    rpy_tar.y = basepos.arrValue[3] * PM_PI / 180.0;
    rpy_tar.p = basepos.arrValue[4] * PM_PI / 180.0;
    rpy_tar.r = basepos.arrValue[5] * PM_PI / 180.0;
    pmRpyQuatConvert(&rpy_tar, &pose_tar.rot);

    PmPose pose_base = pose_tar;
    PmPose pose_delt;
    PmHomogeneous hom_delt;
    PmCartesian rotDir = {0, 1, 0};
    pmRotArbitAxisWithAngleConvert(&(pose_base.tran), &rotDir, delt_ry * PM_PI / 180.0, &hom_delt);
    pmHomPoseConvert(&hom_delt, &pose_delt);
    pmPosePoseMult(&pose_delt, &pose_base, &pose_tar);
    pmQuatRpyConvert(&pose_tar.rot, &rpy_tar);
    pose_tar.tran.x += delt_x;
    pose_tar.tran.y += 0;
    pose_tar.tran.z += delt_z;

    value_ptr->arrValue[0] = pose_tar.tran.x;
    value_ptr->arrValue[1] = pose_tar.tran.y;
    value_ptr->arrValue[2] = pose_tar.tran.z;
    value_ptr->arrValue[3] = rpy_tar.y * 180.0 / PM_PI;
    value_ptr->arrValue[4] = rpy_tar.p * 180.0 / PM_PI;
    value_ptr->arrValue[5] = rpy_tar.r * 180.0 / PM_PI;

    // rtapi_print("The result of postrans is: %f, %f, %f, %f, %f, %f\n",
    // 	value_ptr->arrValue[0], value_ptr->arrValue[1], value_ptr->arrValue[2], value_ptr->arrValue[3], value_ptr->arrValue[4], value_ptr->arrValue[5]);

    *counter = strlen(line);
    return INTERP_OK;
}

/****************************************************************************/
/*! read_h

 Returned Value: int
 If read_integer_value returns an error code, this returns that code.
 If any of the following errors occur, this returns the error code shown.
 Otherwise, it returns INTERP_OK.
 1. The first character read is not h:
 NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED
 2. An h_number has already been inserted in the block:
 NCE_MULTIPLE_H_WORDS_ON_ONE_LINE
 3. The value is negative: NCE_NEGATIVE_H_WORD_TOOL_LENGTH_OFFSET_INDEX_USED
 4. The value is greater than _setup.tool_max:
 NCE_TOOL_LENGTH_OFFSET_INDEX_TOO_BIG

 Side effects:
 counter is reset to the character following the h_number.
 An h_number is inserted in the block.

 Called by: read_one_item

 When this function is called, counter is pointing at an item on the
 line that starts with the character 'h', indicating a tool length
 offset index.  The function reads characters which give the (integer)
 value of the tool length offset index (not the actual distance of the
 offset).

 */

int Interp::read_h(char* line,           //!< string: line of RS274/NGC code being processed
                   int* counter,         //!< pointer to a counter for position on the line
                   block_pointer block,  //!< pointer to a block being filled from the line
                   double* parameters)   //!< array of system parameters
{
    int value;

    CHKS((line[*counter] != 'h'), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = (*counter + 1);
    CHKS((block->h_flag), INTERP_ERR_SYNTAX_FAILED, NCE_MULTIPLE_H_WORDS_ON_ONE_LINE);
    CHP(read_integer_value(line, counter, &value, parameters));
    CHKS((value < -1), INTERP_ERR_SYNTAX_FAILED, NCE_NEGATIVE_H_WORD_USED);
    block->h_flag = true;
    block->h_number = value;
    return INTERP_OK;
}

/****************************************************************************/

/*! read_i

 Returned Value: int
 If read_real_value returns an error code, this returns that code.
 If any of the following errors occur, this returns the error code shown.
 Otherwise, it returns INTERP_OK.
 1. The first character read is not i:
 NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED
 2. An i_coordinate has already been inserted in the block:
 NCE_MULTIPLE_I_WORDS_ON_ONE_LINE

 Side effects:
 counter is reset.
 The i_flag in the block is turned on.
 An i_coordinate setting is inserted in the block.

 Called by: read_one_item

 When this function is called, counter is pointing at an item on the
 line that starts with the character 'i', indicating a i_coordinate
 setting. The function reads characters which tell how to set the
 coordinate, up to the start of the next item or the end of the line.
 This information is inserted in the block. The counter is then set to
 point to the character following.

 The value may be a real number or something that evaluates to a
 real number, so read_real_value is used to read it. Parameters
 may be involved.

 */

int Interp::read_i(char* line,           //!< string: line of RS274 code being processed
                   int* counter,         //!< pointer to a counter for position on the line
                   block_pointer block,  //!< pointer to a block being filled from the line
                   double* parameters)   //!< array of system parameters
{
    jaka_value value;

    CHKS((line[*counter] != 'i'), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = (*counter + 1);
    CHKS((block->i_flag), INTERP_ERR_SYNTAX_FAILED, NCE_MULTIPLE_I_WORDS_ON_ONE_LINE);
    CHP(read_real_value(line, counter, &value, parameters));
    block->i_flag = true;
    block->i_number = value.scalarValue;
    return INTERP_OK;
}

/****************************************************************************/

/*! read_integer_unsigned

 Returned Value: int
 If any of the following errors occur, this returns the error shown.
 Otherwise, INTERP_OK is returned.
 1. The first character is not a digit: NCE_BAD_FORMAT_UNSIGNED_INTEGER
 2. sscanf fails: NCE_SSCANF_FAILED

 Side effects:
 The number read from the line is put into what integer_ptr points at.

 Called by: read_n_number

 This reads an explicit unsigned (positive) integer from a string,
 starting from the position given by *counter. It expects to find one
 or more digits. Any character other than a digit terminates reading
 the integer. Note that if the first character is a sign (+ or -),
 an error will be reported (since a sign is not a digit).

 */

int Interp::read_integer_unsigned(char* line,        //!< string: line of RS274 code being processed
                                  int* counter,      //!< pointer to a counter for position on the line
                                  int* integer_ptr)  //!< pointer to the value being read
{
    int n;
    char c;

    for (n = *counter;; n++)
    {
        c = line[n];
        if ((c < 48) || (c > 57))
            break;
    }
    CHKS((n == *counter), INTERP_ERR_SYNTAX_FAILED, NCE_BAD_FORMAT_UNSIGNED_INTEGER);
    if (sscanf(line + *counter, "%d", integer_ptr) == 0)
        ERS(INTERP_ERR_INTERNAL, NCE_SSCANF_FAILED);
    *counter = n;
    return INTERP_OK;
}

/****************************************************************************/

/*! read_integer_value

 Returned Value: int
 If read_real_value returns an error code, this returns that code.
 If any of the following errors occur, this returns the error code shown.
 Otherwise, it returns INTERP_OK.
 1. The returned value is not close to an integer:
 NCE_NON_INTEGER_VALUE_FOR_INTEGER

 Side effects:
 The number read from the line is put into what integer_ptr points at.

 Called by:
 read_d
 read_l
 read_h
 read_m
 read_parameter
 read_parameter_setting
 read_t

 This reads an integer (positive, negative or zero) from a string,
 starting from the position given by *counter. The value being
 read may be written with a decimal point or it may be an expression
 involving non-integers, as long as the result comes out within 0.0001
 of an integer.

 This proceeds by calling read_real_value and checking that it is
 close to an integer, then returning the integer it is close to.

 */

int Interp::read_integer_value(char* line,          //!< string: line of RS274/NGC code being processed
                               int* counter,        //!< pointer to a counter for position on the line
                               int* integer_ptr,    //!< pointer to the value being read
                               double* parameters)  //!< array of system parameters
{
    jaka_value float_value;
    int ret;

    ret = read_real_value(line, counter, &float_value, parameters);
    CHK_INTERP_READ(ret, line, counter);

    *integer_ptr = (int)floor(float_value.scalarValue);
    if ((float_value.scalarValue - *integer_ptr) > 0.9999)
    {
        *integer_ptr = (int)ceil(float_value.scalarValue);
    }
    else if ((float_value.scalarValue - *integer_ptr) > 0.0001)
        ERS(INTERP_ERR_NON_INTEGER_FOR_INTEGER, NCE_NON_INTEGER_VALUE_FOR_INTEGER);
    return INTERP_OK;
}

bool Interp::threadMutexState()
{
    for (int i = 0; i < MAX_SUB_THREAD; ++i)
    {
        if (threadInterpList[i].is_mutex)
        {
            return true;
        }
    }
    if (is_mutex)
        return true;
    return false;
}

/****************************************************************************/

/*! read_items

 Returned Value: int
 If read_n_number or read_one_item returns an error code,
 this returns that code.
 Otherwise, it returns INTERP_OK.

 Side effects:
 One line of RS274 code is read and data inserted into a block.
 The counter which is passed around among the readers is initialized.
 System parameters may be reset.

 Called by: parse_line

 */

int Interp::read_items(block_pointer block,  //!< pointer to a block being filled from the line
                       char* line,           //!< string: line of RS274/NGC code being processed
                       double* parameters,   //!< array of system parameters
                       setup_pointer settings)
{
    int counter;
    int length;
    int ret;

    length = strlen(line);
    counter = 0;

    // try to parse control statement
    ret = read_o(line, &counter, block, parameters);
    if (ret != INTERP_NOT_HANDLE)
    {
        CHK_INTERP_READ(ret, line, &counter);
        if (_setup.skipping_o)
            return INTERP_OK;
        // comments may follow the statement
        for (; counter < length;)
        {
            if (line[counter] == ';')
                read_semicolon(line, &counter, block, parameters);
            else if (line[counter] == '#')
                read_comment(line, &counter, block, parameters);
            else
                ERS(INTERP_ERR_UNEXPECT_CHARACTER_AFTER_O_WORD, "Unexpected character after O-word");
        }
        if (ret == INTERP_ERROR)
        {
            return ret;
        }
        return INTERP_OK;
    }
    if (_setup.skipping_o)
    {
        return INTERP_OK;
    }

    if (isToken(line, &counter, "admit_enable"))
    {
        if (!m_threadId)
        {
            ret = read_admitenable(line, &counter, block, parameters);
            CHK_INTERP_READ(ret, line, &counter);
        }
        else
            return INTERP_OK;
    }
    else if (isToken(line, &counter, "movj"))
    {
        if (!m_threadId)
        {
            ret = read_movj(line, &counter, block, parameters);
            CHK_INTERP_READ(ret, line, &counter);
        }
        else
            return INTERP_OK;
    }
    else if (isToken(line, &counter, "movl"))
    {
        if (!m_threadId)
        {
            ret = read_movl(line, &counter, block, parameters);
            CHK_INTERP_READ(ret, line, &counter);
        }
        else
            return INTERP_OK;
    }
    else if (isToken(line, &counter, "movc"))
    {
        if (!m_threadId)
        {
            ret = read_movc(line, &counter, block, parameters);
            CHK_INTERP_READ(ret, line, &counter);
        }
        else
            return INTERP_OK;
    }
    else if (isToken(line, &counter, "movs"))
    {
        if (!m_threadId)
        {
            ret = read_movs(line, &counter, block, parameters);
            CHK_INTERP_READ(ret, line, &counter);
        }
        else
            return INTERP_OK;
    }
    else if (isToken(line, &counter, "execute_script"))
    {
        return read_execute_script(line, &counter, block, settings);
    }
    else if (isToken(line, &counter, "subend"))
    {
        return scripts_syncy_parse_execute(line, &counter, block, settings);
    }
    else if (isToken(line, &counter, "sleep"))
    {
        ret = read_sleep(line, &counter, block, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "set_tool_id"))
    {
        if (!m_threadId)
        {
            ret = read_settoolid(line, &counter, block, parameters);
            CHK_INTERP_READ(ret, line, &counter);
        }
        else
            return INTERP_OK;
    }
    else if (isToken(line, &counter, "set_tool"))
    {
        if (!m_threadId)
        {
            ret = read_settool(line, &counter, block, parameters);
            CHK_INTERP_READ(ret, line, &counter);
        }
        else
            return INTERP_OK;
    }
    else if (isToken(line, &counter, "set_user_frame_id"))
    {
        if (!m_threadId)
        {
            ret = read_setuserid(line, &counter, block, parameters);
            CHK_INTERP_READ(ret, line, &counter);
        }
        else
            return INTERP_OK;
    }
    else if (isToken(line, &counter, "set_user_frame"))
    {
        if (!m_threadId)
        {
            ret = read_setuserframe(line, &counter, block, parameters);
            CHK_INTERP_READ(ret, line, &counter);
        }
        else
            return INTERP_OK;
    }
    else if (isToken(line, &counter, "set_payload"))
    {
        if (!m_threadId)
        {
            ret = read_setpayload(line, &counter, block, parameters);
            CHK_INTERP_READ(ret, line, &counter);
        }
        else
            return INTERP_OK;
    }
    else if (isToken(line, &counter, "set_compliance_ft_config"))
    {
        if (!m_threadId)
        {
            ret = read_setadmitctrlconfig(line, &counter, block, parameters);
            CHK_INTERP_READ(ret, line, &counter);
        }
        else
            return INTERP_OK;
    }
    else if (isToken(line, &counter, "set_end_force_condition"))
    {
        if (!m_threadId)
        {
            ret = read_setendforcecondition(line, &counter, block, parameters);
            CHK_INTERP_READ(ret, line, &counter);
        }
        else
            return INTERP_OK;
    }
    else if (isToken(line, &counter, "enable_force_control"))
    {
        if (!m_threadId)
        {
            ret = read_setcomplianttype(line, &counter, block, parameters);
            CHK_INTERP_READ(ret, line, &counter);
        }
        else
            return INTERP_OK;
    }
    else if (isToken(line, &counter, "set_force_control_frame"))
    {
        if (!m_threadId)
        {
            ret = read_setforcectrlframe(line, &counter, block, parameters);
            CHK_INTERP_READ(ret, line, &counter);
        }
        else
            return INTERP_OK;
    }
    else if (isToken(line, &counter, "disable_force_control"))
    {
        if (!m_threadId)
        {
            ret = read_disableforcecontrol(line, &counter, block, parameters);
            CHK_INTERP_READ(ret, line, &counter);
        }
        else
            return INTERP_OK;
    }
    else if (isToken(line, &counter, "set_compliance_velocity_level"))
    {
        if (!m_threadId)
        {
            ret = read_setvelcompliantlevel(line, &counter, block, parameters);
            CHK_INTERP_READ(ret, line, &counter);
        }
        else
            return INTERP_OK;
    }
    else if (isToken(line, &counter, "set_compliance_condition"))
    {
        if (!m_threadId)
        {
            ret = read_setcompliantctrlcondition(line, &counter, block, parameters);
            CHK_INTERP_READ(ret, line, &counter);
        }
        else
            return INTERP_OK;
    }
    else if (isToken(line, &counter, "set_collision_level"))
    {
        if (!m_threadId)
        {
            ret = read_setcollisionlevel(line, &counter, block, parameters);
            CHK_INTERP_READ(ret, line, &counter);
        }
        else
            return INTERP_OK;
    }
    else if (isToken(line, &counter, "set_digital_output"))
    {
        ret = read_setdout(line, &counter, block, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "set_analog_output"))
    {
        ret = read_setaout(line, &counter, block, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "socket_close"))
    {
        ret = read_socketclose(line, &counter, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "enable_conveyor_linear_track"))
    {
        if (!m_threadId)
        {
            ret = read_convey_linear_enable(line, &counter, block, parameters);
            CHK_INTERP_READ(ret, line, &counter);
        }
        else
            return INTERP_OK;
    }
    else if (isToken(line, &counter, "enable_conveyor_circular_track"))
    {
        if (!m_threadId)
        {
            ret = read_convey_circular_enable(line, &counter, block, parameters);
            CHK_INTERP_READ(ret, line, &counter);
        }
        else
            return INTERP_OK;
    }
    else if (isToken(line, &counter, "disable_convayor_track"))
    {
        if (!m_threadId)
        {
            ret = read_convey_disable(line, &counter, block, parameters);
            CHK_INTERP_READ(ret, line, &counter);
        }
        else
            return INTERP_OK;
    }
    else if (isToken(line, &counter, "wait_input"))
    {
        ret = read_waitinput(line, &counter, block, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "enable_speed_override"))
    {
        if (!m_threadId)
        {
            ret = read_override_speed_par(line, &counter);
            CHK_INTERP_READ(ret, line, &counter);
        }
        else
            return INTERP_OK;
    }
    else if (isToken(line, &counter, "disable_speed_override"))
    {
        if (!m_threadId)
        {
            ret = read_stop_override_speed(line, &counter);
            CHK_INTERP_READ(ret, line, &counter);
        }
        else
            return INTERP_OK;
    }
    else if (isToken(line, &counter, "enable_planner_override"))
    {
        ret = read_set_planner_type(line, &counter);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "disable_planner_override"))
    {
        ret = read_close_speed_planner(line, &counter);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "log_message"))
    {
        ret = read_logmsg(line, &counter, block, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "pause"))
    {
        ret = read_pause(line, &counter, block, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "resume"))
    {
        ret = read_resume(line, &counter, block, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "exit"))
    {
        ret = read_exit(line, &counter, block, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "tio_update_signal"))
    {
        ret = read_tioupdatesignal(line, &counter, block, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "tio_send_command"))
    {
        ret = read_tiosendcommand(line, &counter, block, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "create_thread"))
    {
        if (m_multi_thread_flag)
            ret = read_threadcreate(line, &counter, block, parameters);
        else
            return INTERP_OK;

        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "thread_destroy"))
    {
        if (m_multi_thread_flag)
            ret = read_threaddestroy(line, &counter, block, parameters);
        else
            return INTERP_OK;
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "thread_join"))
    {
        if (m_multi_thread_flag)
            ret = read_threadjoin(line, &counter, block, parameters);
        else
            return INTERP_OK;
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "thread_enter_critical"))
    {
        if (m_multi_thread_flag)
            ret = read_threadentermutex(line, &counter, block, parameters);
        else
            return INTERP_OK;
        CHK_INTERP_READ(ret, line, &counter);
        return ret;
    }
    else if (isToken(line, &counter, "thread_exit_critical"))
    {
        if (m_multi_thread_flag)
            ret = read_threadexitmutex(line, &counter, block, parameters);
        else
            return INTERP_OK;
        CHK_INTERP_READ(ret, line, &counter);
        return ret;
    }
    // function call without return value
    else if (isToken(line, &counter, "atan2"))
    {
        ret = read_atan2(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "pose_add"))
    {
        ret = read_poseadd(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "pose_sub"))
    {
        ret = read_posesub(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "pose_inv"))
    {
        ret = read_poseinv(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "pose_dist"))
    {
        ret = read_posedist(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "pose_trans"))
    {
        ret = read_posetrans(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "pose_intpl"))
    {
        ret = read_poseintpl(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "kine_inverse"))
    {
        ret = read_kinInverse(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "kine_forward"))
    {
        ret = read_kinForward(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "is_pose_reachable"))
    {
        ret = read_isPoseReachable(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "xy_plane_trans"))
    {
        ret = read_xyplanetrans(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "yz_plane_trans"))
    {
        ret = read_yzplanetrans(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "zx_plane_trans"))
    {
        ret = read_zxplanetrans(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "get_digital_output"))
    {
        ret = read_getdout(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "get_analog_output"))
    {
        ret = read_getaout(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "get_digital_input"))
    {
        ret = read_getdin(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "get_analog_input"))
    {
        ret = read_getain(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "get_atl_joint_pose"))
    {
        ret = read_getatljointpose(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "get_atl_tcp_pose"))
    {
        ret = read_getatltcppose(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "get_atl_flange_pose"))
    {
        ret = read_getatlflangepose(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "get_tool_offsets_of"))
    {
        ret = read_gettooloffsetsof(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "get_tool_offsets"))
    {
        ret = read_gettooloffsets(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "get_user_frame_of"))
    {
        ret = read_getuseroffsetsof(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "get_user_frame"))
    {
        ret = read_getuseroffsets(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "get_collision_level"))
    {
        ret = read_getclsnsensitivity(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "get_payload"))
    {
        ret = read_getpayload(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "get_sensor_torque"))
    {
        ret = read_getsensortorque(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "get_timeout"))
    {
        ret = read_gettimeout(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "get_length"))
    {
        ret = read_getlength(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "get_array_from_string"))
    {
        ret = read_getarrayfromstring(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "get_string_from_array"))
    {
        ret = read_getstringfromarray(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "socket_open"))
    {
        ret = read_socketopen(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "socket_get_var"))
    {
        ret = read_socketgetvar(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "socket_recv"))
    {
        ret = read_socketrecv(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "socket_read_real"))
    {
        ret = read_socketreadreal(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "socket_read_string"))
    {
        ret = read_socketreadstring(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "socket_send"))
    {
        ret = read_socketsend(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "socket_send_byte"))
    {
        ret = read_socketsendbyte(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "sprintf"))
    {
        ret = read_sprintf(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "sscanf"))
    {
        ret = read_sscanf(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "string_concat"))
    {
        ret = read_stringconcat(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "strcmp"))
    {
        ret = read_strcmp(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "get_system_clock"))
    {
        ret = read_getsysclock(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    else if (isToken(line, &counter, "tio_get_signal_value"))
    {
        ret = read_tiogetsignalvalue(line, &counter, nullptr, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }

    /*end of reading built-in functions*/

    // try to match a new parameter assignment
    std::string s(line);
    std::smatch sm;
    if (std::regex_match(s, sm, std::regex("^[A-Za-z_][A-Za-z0-9_\\[\\]]*\\s*=.*")))
    {
        ret = read_parameter_setting(line, &counter, block, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }

    for (; counter < length;)
    {
        ret = read_one_item(line, &counter, block, parameters);
        CHK_INTERP_READ(ret, line, &counter);
    }
    return INTERP_OK;
}

/****************************************************************************/

/*! read_j

 Returned Value: int
 If read_real_value returns an error code, this returns that code.
 If any of the following errors occur, this returns the error code shown.
 Otherwise, it returns INTERP_OK.
 1. The first character read is not j:
 NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED
 2. A j_coordinate has already been inserted in the block.
 NCE_MULTIPLE_J_WORDS_ON_ONE_LINE

 Side effects:
 counter is reset.
 The j_flag in the block is turned on.
 A j_coordinate setting is inserted in the block.

 Called by: read_one_item

 When this function is called, counter is pointing at an item on the
 line that starts with the character 'j', indicating a j_coordinate
 setting. The function reads characters which tell how to set the
 coordinate, up to the start of the next item or the end of the line.
 This information is inserted in the block. The counter is then set to
 point to the character following.

 The value may be a real number or something that evaluates to a real
 number, so read_real_value is used to read it. Parameters may be
 involved.

 */

int Interp::read_j(char* line,           //!< string: line of RS274 code being processed
                   int* counter,         //!< pointer to a counter for position on the line
                   block_pointer block,  //!< pointer to a block being filled from the line
                   double* parameters)   //!< array of system parameters
{
    jaka_value value;

    CHKS((line[*counter] != 'j'), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = (*counter + 1);
    CHKS((block->j_flag), INTERP_ERR_SYNTAX_FAILED, NCE_MULTIPLE_J_WORDS_ON_ONE_LINE);
    CHP(read_real_value(line, counter, &value, parameters));
    block->j_flag = true;
    block->j_number = value.scalarValue;
    return INTERP_OK;
}

/****************************************************************************/

/*! read_k

 Returned Value: int
 If read_real_value returns an error code, this returns that code.
 If any of the following errors occur, this returns the error code shown.
 Otherwise, it returns INTERP_OK.
 1. The first character read is not k:
 NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED
 2. A k_coordinate has already been inserted in the block:
 NCE_MULTIPLE_K_WORDS_ON_ONE_LINE

 Side effects:
 counter is reset.
 The k_flag in the block is turned on.
 A k_coordinate setting is inserted in the block.

 Called by: read_one_item

 When this function is called, counter is pointing at an item on the
 line that starts with the character 'k', indicating a k_coordinate
 setting. The function reads characters which tell how to set the
 coordinate, up to the start of the next item or the end of the line.
 This information is inserted in the block. The counter is then set to
 point to the character following.

 The value may be a real number or something that evaluates to a real
 number, so read_real_value is used to read it. Parameters may be
 involved.

 */

int Interp::read_k(char* line,           //!< string: line of RS274 code being processed
                   int* counter,         //!< pointer to a counter for position on the line
                   block_pointer block,  //!< pointer to a block being filled from the line
                   double* parameters)   //!< array of system parameters
{
    jaka_value value;

    CHKS((line[*counter] != 'k'), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = (*counter + 1);
    CHKS((block->k_flag), INTERP_ERR_SYNTAX_FAILED, NCE_MULTIPLE_K_WORDS_ON_ONE_LINE);
    CHP(read_real_value(line, counter, &value, parameters));
    block->k_flag = true;
    block->k_number = value.scalarValue;
    return INTERP_OK;
}

/****************************************************************************/

/*! read_l

 Returned Value: int
 If read_integer_value returns an error code, this returns that code.
 If any of the following errors occur, this returns the error code shown.
 Otherwise, it returns INTERP_OK.
 1. The first character read is not l:
 NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED
 2. An l_number has already been inserted in the block:
 NCE_MULTIPLE_L_WORDS_ON_ONE_LINE
 3. the l_number is negative: NCE_NEGATIVE_L_WORD_USED

 Side effects:
 counter is reset to the character following the l number.
 An l code is inserted in the block as the value of l.

 Called by: read_one_item

 When this function is called, counter is pointing at an item on the
 line that starts with the character 'l', indicating an L code.
 The function reads characters which give the (integer) value of the
 L code.

 L codes are used for:
 1. the number of times a canned cycle should be repeated.
 2. a key with G10.

 */

int Interp::read_l(char* line,           //!< string: line of RS274/NGC code being processed
                   int* counter,         //!< pointer to a counter for position on the line
                   block_pointer block,  //!< pointer to a block being filled from the line
                   double* parameters)   //!< array of system parameters
{
    int value;

    CHKS((line[*counter] != 'l'), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = (*counter + 1);
    CHKS((block->l_number > -1), INTERP_ERR_SYNTAX_FAILED, NCE_MULTIPLE_L_WORDS_ON_ONE_LINE);
    CHP(read_integer_value(line, counter, &value, parameters));
    CHKS((value < 0), INTERP_ERR_SYNTAX_FAILED, NCE_NEGATIVE_L_WORD_USED);
    block->l_number = value;
    block->l_flag = true;
    return INTERP_OK;
}

/****************************************************************************/

/*! read_n_number

 Returned Value: int
 If read_integer_unsigned returns an error code, this returns that code.
 If any of the following errors occur, this returns the error code shown.
 Otherwise, it returns INTERP_OK.
 1. The first character read is not n:
 NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED
 2. The line number is too large (more than 99999):
 NCE_LINE_NUMBER_GREATER_THAN_99999

 Side effects:
 counter is reset to the character following the line number.
 A line number is inserted in the block.

 Called by: read_items

 When this function is called, counter is pointing at an item on the
 line that starts with the character 'n', indicating a line number.
 The function reads characters which give the (integer) value of the
 line number.

 Note that extra initial zeros in a line number will not cause the
 line number to be too large.

 */

int Interp::read_n_number(char* line,           //!< string: line of RS274    code being processed
                          int* counter,         //!< pointer to a counter for position on the line
                          block_pointer block)  //!< pointer to a block being filled from the line
{
    int value;

    CHKS(((line[*counter] != 'n') && (line[*counter] != 'o')), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = (*counter + 1);
    CHP(read_integer_unsigned(line, counter, &value));
    /* This next test is problematic as many CAM systems will exceed this !
	 CHKS((value > 99999), INTERP_ERR_SYNTAX_FAILED, NCE_LINE_NUMBER_GREATER_THAN_99999); */
    block->n_number = value;

    // accept & ignore fractional line numbers
    if (line[*counter] == '.')
    {
        *counter = (*counter + 1);
        CHP(read_integer_unsigned(line, counter, &value));
    }
    return INTERP_OK;
}

/****************************************************************************/

/*! read_m

 Returned Value:
 If read_integer_value returns an error code, this returns that code.
 If any of the following errors occur, this returns the error code shown.
 Otherwise, it returns INTERP_OK.
 1. The first character read is not m:
 NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED
 2. The value is negative: NCE_NEGATIVE_M_CODE_USED
 3. The value is greater than 199: NCE_M_CODE_GREATER_THAN_199
 4. The m code is not known to the system: NCE_UNKNOWN_M_CODE_USED
 5. Another m code in the same modal group has already been read:
 NCE_TWO_M_CODES_USED_FROM_SAME_MODAL_GROUP

 Side effects:
 counter is reset to the character following the m number.
 An m code is inserted as the value of the appropriate mode in the
 m_modes array in the block.
 The m code counter in the block is increased by 1.

 Called by: read_one_item

 When this function is called, counter is pointing at an item on the
 line that starts with the character 'm', indicating an m code.
 The function reads characters which give the (integer) value of the
 m code.

 read_integer_value allows a minus sign, so a check for a negative value
 is needed here, and the parameters argument is also needed.

 */

int Interp::read_m(char* line,           //!< string: line of RS274 code being processed
                   int* counter,         //!< pointer to a counter for position on the line
                   block_pointer block,  //!< pointer to a block being filled from the line
                   double* parameters)   //!< array of system parameters
{
    int value;
    int mode;

    CHKS((line[*counter] != 'm'), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = (*counter + 1);
    CHP(read_integer_value(line, counter, &value, parameters));
    CHKS((value < 0), INTERP_ERR_SYNTAX_FAILED, NCE_NEGATIVE_M_CODE_USED);

    remap_pointer r = _setup.m_remapped[value];
    if (r)
    {
        mode = r->modal_group;
        CHKS((mode < 0), INTERP_ERR_SYNTAX_FAILED, "BUG: M remapping: modal group < 0");
        CHKS((mode > 10), INTERP_ERR_SYNTAX_FAILED, "BUG: M remapping: modal group > 10");

        CHKS((block->m_modes[mode] != -1), INTERP_ERR_SYNTAX_FAILED, NCE_TWO_M_CODES_USED_FROM_SAME_MODAL_GROUP);
        block->m_modes[mode] = value;
        block->m_count++;
        return INTERP_OK;
    }

    CHKS((value > 199), INTERP_ERR_SYNTAX_FAILED, NCE_M_CODE_GREATER_THAN_199, value);
    mode = _ems[value];
    CHKS((mode == -1), INTERP_ERR_SYNTAX_FAILED, NCE_UNKNOWN_M_CODE_USED, value);
    CHKS((block->m_modes[mode] != -1), INTERP_ERR_SYNTAX_FAILED, NCE_TWO_M_CODES_USED_FROM_SAME_MODAL_GROUP);
    block->m_modes[mode] = value;
    block->m_count++;
    if (value >= 100 && value < 200)
    {
        block->user_m = 1;
    }
    return INTERP_OK;
}

/****************************************************************************/

/*! read_one_item

 Returned Value: int
 If a reader function which is called returns an error code, that
 error code is returned.
 If any of the following errors occur, this returns the error code shown.
 Otherwise, it returns INTERP_OK.
 1. the first character read is not a known character for starting a
 word: NCE_BAD_CHARACTER_USED

 Side effects:
 This function reads one item from a line of RS274/NGC code and inserts
 the information in a block. System parameters may be reset.

 Called by: read_items.

 When this function is called, the counter is set so that the position
 being considered is the first position of a word. The character at
 that position must be one known to the system.  In this version those
 characters are: a,b,c,d,f,g,h,i,j,k,l,m,n,p,q,r,s,t,x,y,z,(,#,;.
 However, read_items calls read_n_number directly if the first word
 begins with n, so no read function is included in the "_readers" array
 for the letter n. Thus, if an n word is encountered in the middle of
 a line, this function reports NCE_BAD_CHARACTER_USED.

 The function looks for a letter or special character and matches it to
 a pointer to a function in _readers[] - The position of the function
 pointers in the array match their ASCII number.
 Once the character has been matched, the function calls a selected function
 according to what the letter or character is.  The selected function will
 be responsible to consider all the characters that comprise the remainder
 of the item, and reset the pointer so that it points to the next character
 after the end of the item (which may be the end of the line or the first
 character of another item).

 After an item is read, the counter is set at the index of the
 next unread character. The item data is stored in the block.

 It is expected that the format of a comment will have been checked;
 this is being done by close_and_downcase. Bad format comments will
 have prevented the system from getting this far, so that this function
 can assume a close parenthesis will be found when an open parenthesis
 has been found, and that comments are not nested.

 */

int Interp::read_one_item(char* line,           //!< string: line of RS274/NGC code being processed
                          int* counter,         //!< pointer to a counter for position on the line
                          block_pointer block,  //!< pointer to a block being filled from the line
                          double* parameters)   /* array of system parameters  */
{
    read_function_pointer function_pointer;
    char letter;

    letter = line[*counter]; /* check if in array range */
    CHKS(((letter < ' ') || (letter > 'z')), INTERP_ERR_SYNTAX_FAILED, _("Bad character '\\%03o' used"), (unsigned char)letter);
    function_pointer = _readers[(int)letter]; /* Find the function pointer in the array */
    CHKS((function_pointer == 0),
         INTERP_ERR_SYNTAX_FAILED,
         (!isprint(letter) || isspace(letter)) ? _("Bad character '\\%03o' used") : _("Bad character '%c' used"),
         letter);
    CHP((*this.*function_pointer)(line, counter, block, parameters)); /* Call the function */
    return INTERP_OK;
}

/****************************************************************************/

/*! read_operation

 Returned Value: int
 If any of the following errors occur, this returns the error code shown.
 Otherwise, it returns INTERP_OK.
 1. The operation is unknown:
 NCE_UNKNOWN_OPERATION_NAME_STARTING_WITH_A
 NCE_UNKNOWN_OPERATION_NAME_STARTING_WITH_M
 NCE_UNKNOWN_OPERATION_NAME_STARTING_WITH_O
 NCE_UNKNOWN_OPERATION_NAME_STARTING_WITH_X
 NCE_UNKNOWN_OPERATION
 2. The line ends without closing the expression: NCE_UNCLOSED_EXPRESSION

 Side effects:
 An integer representing the operation is put into what operation points
 at.  The counter is reset to point to the first character after the
 operation.

 Called by: read_real_expression

 This expects to be reading a binary operation (+, -, /, *, **, and,
 mod, or, xor) or a right bracket (]). If one of these is found, the
 value of operation is set to the symbolic value for that operation.
 If not, an error is reported as described above.

 */

int Interp::read_operation(char* line,      //!< string: line of RS274/NGC code being processed
                           int* counter,    //!< pointer to a counter for position on the line
                           int* operation)  //!< pointer to operation to be read
{
    char c = line[*counter];
    switch (c)
    {
    case '+':
        *operation = PLUS;
        break;
    case '-':
        *operation = MINUS;
        break;
    case '/':
        *operation = DIVIDED_BY;
        break;
    case '*':
        if ((*counter + 1) < (int)strlen(line) && line[*counter + 1] == '*')
        {
            *operation = POWER;
            *counter = (*counter + 1);
        }
        else
        {
            *operation = TIMES;
        }
        break;
    case ')':
        *operation = RIGHT_BRACKET;
        break;
    case ',':
        *operation = RIGHT_COMMA;
        break;
    case ']':
        *operation = RIGHT_SBRACKET;
        break;
    case '&':
        if ((*counter + 1) < (int)strlen(line) && line[*counter + 1] == '&')
        {
            *operation = AND2;
            *counter = (*counter + 1);
        }
        else
            ERS(INTERP_ERR_UNKNOWN_OPERATION_NAME_STARTING, NCE_UNKNOWN_OPERATION_NAME_STARTING_WITH_A);
        break;
    case '%':
        *operation = MODULO;
        break;
    case '|':
        if ((*counter + 1) < (int)strlen(line) && line[*counter + 1] == '|')
        {
            *operation = NON_EXCLUSIVE_OR;
            *counter = (*counter + 1);
        }
        else
            ERS(INTERP_ERR_UNKNOWN_OPERATION_NAME_STARTING, NCE_UNKNOWN_OPERATION_NAME_STARTING_WITH_O);
        break;
    case '^':
        *operation = EXCLUSIVE_OR;
        break;
        /* relational operators */
    case '=':
        if ((*counter + 1) < (int)strlen(line) && line[*counter + 1] == '=')
        {
            *operation = EQ;
            *counter = (*counter + 1);
        }
        else
            ERS(INTERP_ERR_UNKNOWN_OPERATION_NAME_STARTING, NCE_UNKNOWN_OPERATION_NAME_STARTING_WITH_E);
        break;
    case '!':
        if ((*counter + 1) < (int)strlen(line) && line[*counter + 1] == '=')
        {
            *operation = NE;
            *counter = (*counter + 1);
        }
        else
            ERS(INTERP_ERR_UNKNOWN_OPERATION_NAME_STARTING, NCE_UNKNOWN_OPERATION_NAME_STARTING_WITH_N);
        break;
    case '>':
        if ((*counter + 1) < (int)strlen(line) && line[*counter + 1] == '=')
        {
            *operation = GE;
            *counter = (*counter + 1);
        }
        else
        {
            *operation = GT;
        }
        break;
    case '<':
        if ((*counter + 1) < (int)strlen(line) && line[*counter + 1] == '=')
        {
            *operation = LE;
            *counter = (*counter + 1);
        }
        else
        {
            *operation = LT;
        }
        break;
    case 0:
        ERS(INTERP_ERR_UNCLOSED_EXPRESSION, NCE_UNCLOSED_EXPRESSION);
    default:
        ERS(INTERP_ERR_UNKNOWN_OPERATION, NCE_UNKNOWN_OPERATION);
    }

    // move to the next token
    *counter = (*counter + 1);
    return INTERP_OK;
}

/****************************************************************************/

/*! read_operation_unary

 Returned Value: int
 If the operation is not a known unary operation, this returns one of
 the following error codes:
 NCE_UNKNOWN_WORD_STARTING_WITH_A
 NCE_UNKNOWN_WORD_STARTING_WITH_C
 NCE_UNKNOWN_WORD_STARTING_WITH_E
 NCE_UNKNOWN_WORD_STARTING_WITH_F
 NCE_UNKNOWN_WORD_STARTING_WITH_G
 NCE_UNKNOWN_WORD_STARTING_WITH_L
 NCE_UNKNOWN_WORD_STARTING_WITH_R
 NCE_UNKNOWN_WORD_STARTING_WITH_S
 NCE_UNKNOWN_WORD_STARTING_WITH_T
 NCE_UNKNOWN_WORD_WHERE_UNARY_OPERATION_COULD_BE
 Otherwise, this returns INTERP_OK.

 Side effects:
 An integer code for the name of the operation read from the
 line is put into what operation points at.
 The counter is reset to point to the first character after the
 characters which make up the operation name.

 Called by:
 read_unary

 This attempts to read the name of a unary operation out of the line,
 starting at the index given by the counter. Known operations are:
 abs, acos, asin, atan, cos, exp, fix, fup, ln, round, sin, sqrt, tan.

 */

int Interp::read_operation_unary(char* line,      //!< string: line of RS274/NGC code being processed
                                 int* counter,    //!< pointer to a counter for position on the line
                                 int* operation)  //!< pointer to operation to be read
{
    char c = line[*counter];
    switch (c)
    {
    case 'a':
        if (isToken(line, counter, "abs"))
        {
            *operation = ABS;
        }
        else if (isToken(line, counter, "acos"))
        {
            *operation = ACOS;
        }
        else if (isToken(line, counter, "asin"))
        {
            *operation = ASIN;
        }
        else if (isToken(line, counter, "atan2"))
        {
            *operation = ATAN2;
        }
        else if (isToken(line, counter, "atan"))
        {
            *operation = ATAN;
        }
        else
            return INTERP_NOT_HANDLE;
        break;
    case 'c':
        if (isToken(line, counter, "cos"))
        {
            *operation = COS;
        }
        else if (isToken(line, counter, "ceil"))
        {
            *operation = CEIL;
        }
        else
            return INTERP_NOT_HANDLE;
        break;
    case 'e':
        if (isToken(line, counter, "exp"))
        {
            *operation = EXP;
        }
        else if (isToken(line, counter, "exists"))
        {
            *operation = EXISTS;
        }
        else
        {
            return INTERP_NOT_HANDLE;
        }
        break;
    case 'f':
        if (isToken(line, counter, "floor"))
        {
            *operation = FLOOR;
        }
        else
            return INTERP_NOT_HANDLE;
        break;
    case 'i':
        if (isToken(line, counter, "is_pose_reachable"))
        {
            *operation = POSEREACHABLE;
        }
        else
            return INTERP_NOT_HANDLE;
        break;
    case 'k':
        if (isToken(line, counter, "kine_inverse"))
        {
            *operation = KININVERSE;
        }
        else if (isToken(line, counter, "kine_forward"))
        {
            *operation = KINFORWARD;
        }
        else
        {
            return INTERP_NOT_HANDLE;
        }
        break;
    case 'g':
        if (isToken(line, counter, "get_digital_output"))
        {
            *operation = GETDOUT;
        }
        else if (isToken(line, counter, "get_analog_output"))
        {
            *operation = GETAOUT;
        }
        else if (isToken(line, counter, "get_digital_input"))
        {
            *operation = GETDIN;
        }
        else if (isToken(line, counter, "get_analog_input"))
        {
            *operation = GETAIN;
        }
        else if (isToken(line, counter, "get_atl_joint_pose"))
        {
            *operation = GETATLJOINTPOSE;
        }
        else if (isToken(line, counter, "get_atl_tcp_pose"))
        {
            *operation = GETATLTCPPOSE;
        }
        else if (isToken(line, counter, "get_atl_flange_pose"))
        {
            *operation = GETATLFLANGEPOSE;
        }
        else if (isToken(line, counter, "get_tool_offsets_of"))
        {
            *operation = GETTCPOFFSETSOF;
        }
        else if (isToken(line, counter, "get_tool_offsets"))
        {
            *operation = GETTCPOFFSETS;
        }
        else if (isToken(line, counter, "get_user_frame_of"))
        {
            *operation = GETUSEROFFSETSOF;
        }
        else if (isToken(line, counter, "get_user_frame"))
        {
            *operation = GETUSEROFFSETS;
        }
        else if (isToken(line, counter, "get_collision_level"))
        {
            *operation = GETCLSNSENSITIVITY;
        }
        else if (isToken(line, counter, "get_payload"))
        {
            *operation = GETPAYLOAD;
        }
        else if (isToken(line, counter, "get_sensor_torque"))
        {
            *operation = GETSENSORTORQUE;
        }
        else if (isToken(line, counter, "get_length"))
        {
            *operation = GETLENGTH;
        }
        else if (isToken(line, counter, "get_timeout"))
        {
            *operation = GETTIMEOUT;
        }
        else if (isToken(line, counter, "get_array_from_string"))
        {
            *operation = GETARRAYFROMSTRING;
        }
        else if (isToken(line, counter, "get_string_from_array"))
        {
            *operation = GETSTRINGFROMARRAY;
        }
        else if (isToken(line, counter, "get_system_clock"))
        {
            *operation = GETSYSCLOCK;
        }
        else
            return INTERP_NOT_HANDLE;
        break;
    case 'l':
        if (isToken(line, counter, "ln"))
        {
            *operation = LN;
        }
        else
            return INTERP_NOT_HANDLE;
        break;
    case 'p':
        if (isToken(line, counter, "pose_add"))
        {
            *operation = POSEADD;
        }
        else if (isToken(line, counter, "pose_sub"))
        {
            *operation = POSESUB;
        }
        else if (isToken(line, counter, "pose_inv"))
        {
            *operation = POSEINV;
        }
        else if (isToken(line, counter, "pose_dist"))
        {
            *operation = POSEDIST;
        }
        else if (isToken(line, counter, "pose_trans"))
        {
            *operation = POSETRANS;
        }
        else if (isToken(line, counter, "pose_intpl"))
        {
            *operation = POSEINTPL;
        }
        else
            return INTERP_NOT_HANDLE;
        break;
    case 'r':
        if (isToken(line, counter, "round"))
        {
            *operation = ROUND;
        }
        else if (isToken(line, counter, "rv3d_get_position_num"))
        {
            *operation = RV3DGETPOSCNT;
        }
        else if (isToken(line, counter, "rv3d_get_position"))
        {
            *operation = RV3DGETPOS;
        }
        else if (isToken(line, counter, "rv2d_get_position"))
        {
            *operation = VISIONLOC;
        }
        else
            return INTERP_NOT_HANDLE;
        break;
    case 's':
        if (isToken(line, counter, "sin"))
        {
            *operation = SIN;
        }
        else if (isToken(line, counter, "sqrt"))
        {
            *operation = SQRT;
        }
        else if (isToken(line, counter, "socket_open"))
        {
            *operation = SOCKETOPEN;
        }
        else if (isToken(line, counter, "socket_recv"))
        {
            *operation = SOCKETRECV;
        }
        else if (isToken(line, counter, "socket_get_var"))
        {
            *operation = SOCKETGETVAR;
        }
        else if (isToken(line, counter, "socket_read_real"))
        {
            *operation = SOCKETREADREAL;
        }
        else if (isToken(line, counter, "socket_read_string"))
        {
            *operation = SOCKETREADSTRING;
        }
        else if (isToken(line, counter, "socket_send_byte"))
        {
            *operation = SOCKETSENDBYTE;
        }
        else if (isToken(line, counter, "socket_send"))
        {
            *operation = SOCKETSEND;
        }
        else if (isToken(line, counter, "sprintf"))
        {
            *operation = SPRINTF;
        }
        else if (isToken(line, counter, "sscanf"))
        {
            *operation = SSCANF;
        }
        else if (isToken(line, counter, "string_concat"))
        {
            *operation = STRINGCONCAT;
        }
        else if (isToken(line, counter, "strcmp"))
        {
            *operation = STRINGCMP;
        }
        else
            return INTERP_NOT_HANDLE;
        break;
    case 't':
        if (isToken(line, counter, "tan"))
        {
            *operation = TAN;
        }
        else if (isToken(line, counter, "tio_get_signal_value"))
        {
            *operation = GETTIOSIGNALVAL;
        }
        else
            return INTERP_NOT_HANDLE;
        break;
    case 'x':
        if (isToken(line, counter, "xy_plane_trans"))
        {
            *operation = XYPLANETRANS;
        }
        else
            return INTERP_NOT_HANDLE;
        break;
    case 'y':
        if (isToken(line, counter, "yz_plane_trans"))
        {
            *operation = YZPLANETRANS;
        }
        else
            return INTERP_NOT_HANDLE;
        break;
    case 'z':
        if (isToken(line, counter, "zx_plane_trans"))
        {
            *operation = ZXPLANETRANS;
        }
        else
            return INTERP_NOT_HANDLE;
        break;
    default:
        return INTERP_NOT_HANDLE;
    }
    return INTERP_OK;
}

/****************************************************************************/

/* read_o

 Returned Value: int
 If read_real_value returns an error code, this returns that code.
 If any of the following errors occur, this returns the error code shown.
 Otherwise, it returns RS274NGC_OK.
 1. The first character read is not o:
 NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED

 Side effects:
 counter is reset to point to the first character following the p value.
 The p value setting is inserted in block.

 Called by: read_one_item

 When this function is called, counter is pointing at an item on the
 line that starts with the character 'o', indicating a o value
 setting. The function reads characters which tell how to set the o
 value, up to the start of the next item or the end of the line. This
 information is inserted in the block.

 O codes are used for:
 1.  sub
 2.  endsub
 3.  call
 4.  do
 5.  while
 6.  if
 7.  elseif
 8.  else
 9.  endif
 10. break
 11. continue
 12. endwhile
 13. return

 Labels (o-words) are of local scope except for sub, endsub, and call which
 are of global scope.

 Named o-words and scoping of o-words are implemented now.
 Numeric o-words get converted to strings. Global o-words are stored as
 just the string. Local o-words are stored as the name (o-word) of the
 sub containing the o-word, followed by a '#', followed by the local o-word.
 If the local o-word is not contained within a sub, then the sub name is null
 and the o-word just begins with a '#'.

 It has been a pain to do this because o-words are used as integers all over
 the place.
 !!!KL the code could use some cleanup.

 */

/*@brief the new read_o function for new syntax 
 *
 *compared with the old syntax, the o statements are changed,
 *the prefix o is removed so that it will be more concise 
 *@param line line of RS274/NGC code being processed
 *@param counter pointer to a counter for position on the line
 *@param block pointer to a block being filled from the line
 *@param parameters array of system parameters
 *@return the result of line parsing
 */
int Interp::read_o(char* line, int* counter, block_pointer block, double* parameters)
{
    // static char name[] = "read_o";
    double value;
    int param_cnt;
    char oNameBuf[LINELEN + 1];
    const char* subName;
    char fullNameBuf[2 * LINELEN + 1];
    // int oNumber;
    int ret;
    extern const char* o_ops[];

    std::string s(line);
    std::smatch sm;

    // auto CMP = [&](std::string tok, bool hasStatement)-> bool {
    // 	std::string strReg = "\\s*" + tok + "\\b";
    // 	if(hasStatement) {
    // 		strReg += "[^:]*:\\s*(#.*)?";	// take the comment into consideration (#.*)?
    // 	} else {
    // 		strReg += ".*(#.*)?";
    // 	}
    // 	std::regex reg(strReg);
    // 	return std::regex_match(s, sm, reg) && (*counter += tok.size());
    // };

    if (isToken(line, counter, "def"))
    {
        skipBlank(line, counter);
        char subName[256];
        CHP(read_name(line, counter, subName));
        // printf("the read subname is: %s\n", subName);
        std::pair<int, std::string> subItem = std::make_pair(O_sub, std::string(subName));
        m_owordIdMap.push_back(subItem);
        sprintf(oNameBuf, "%s", subItem.second.c_str());
        block->o_type = O_sub;
        *counter = strlen(line);
    }
    else if (isToken(line, counter, "call"))
    {
        char subName[256];
        skipBlank(line, counter);
        CHP(read_name(line, counter, subName));
        sprintf(oNameBuf, "%s", subName);
        block->o_type = O_call;
        *counter = strlen(line);
    }
    else if (isToken(line, counter, "do"))
    {
        _setup.doing_else = 0;
        _setup.doing_elif = 0;
        std::pair<int, std::string> doItem = std::make_pair(O_do, std::to_string(_setup.sequence_number - 1));
        m_owordIdMap.push_back(doItem);
        sprintf(oNameBuf, "%s", doItem.second.c_str());
        block->o_type = O_do;
    }
    else if (isToken(line, counter, "while"))
    {
        _setup.doing_else = 0;
        _setup.doing_elif = 0;
        std::string strId = std::to_string(_setup.sequence_number - 1);
        if (m_owordIdMap.size() == 0 || m_owordIdMap.back().first != O_do)
        {
            std::pair<int, std::string> whileItem = std::make_pair(O_while, strId);
            m_owordIdMap.push_back(whileItem);
        }
        else
        {
            m_owordIdMap.pop_back();
        }
        sprintf(oNameBuf, "%s", strId.c_str());
        block->o_type = O_while;
    }
    else if (isToken(line, counter, "repeat"))
    {
        std::pair<int, std::string> repeatItem = std::make_pair(O_repeat, std::to_string(_setup.sequence_number - 1));
        m_owordIdMap.push_back(repeatItem);
        sprintf(oNameBuf, "%s", repeatItem.second.c_str());
        block->o_type = O_repeat;
    }
    else if (isToken(line, counter, "if"))
    {
        std::pair<int, std::string> ifItem = std::make_pair(O_if, std::to_string(_setup.sequence_number - 1));
        m_owordIdMap.push_back(ifItem);
        sprintf(oNameBuf, "%s", ifItem.second.c_str());
        block->o_type = O_if;
        _setup.doing_else = 0;
        _setup.doing_elif = 0;
    }
    else if (isToken(line, counter, "elif"))
    {
        try
        {
            if (m_owordIdMap.size() <= 0)
            {
                ERS(INTERP_ERR_UNMATCHED_ELIF, "unmatched control oword 'elif'");  // need to match a if statement
            }
            std::pair<int, std::string> lastItem = m_owordIdMap.back();
            if (lastItem.first != O_if)
            {
                ERS(INTERP_ERR_UNMATCHED_ELIF, "unmatched control oword 'elif'");  // need to match a if statement
            }
            sprintf(oNameBuf, "%s", lastItem.second.c_str());
            block->o_type = O_elif;
            _setup.doing_else = 0;
            _setup.doing_elif = 1;
        }
        catch (...)
        {
            ERS(INTERP_ERR_UNMATCHED_ELIF, "unmatched control oword 'elif'");
        }
    }
    else if (isToken(line, counter, "else"))
    {
        try
        {
            if (m_owordIdMap.size() <= 0)
            {
                ERS(INTERP_ERR_UNMATCHED_ELSE, "unmatched control oword 'else'");  // need to match a if statement
            }
            std::pair<int, std::string> lastItem = m_owordIdMap.back();
            if (lastItem.first != O_if)
            {
                ERS(INTERP_ERR_UNMATCHED_ELSE, "unmatched control oword 'else'");  // need to match a if statement
            }
            sprintf(oNameBuf, "%s", lastItem.second.c_str());
            block->o_type = O_else;
            _setup.doing_else = 1;
            _setup.doing_elif = 0;
        }
        catch (...)
        {
            ERS(INTERP_ERR_UNMATCHED_ELSE, "unmatched control oword 'else'");
        }
    }
    else if (isToken(line, counter, "break"))
    {
        try
        {
            std::vector<std::pair<int, std::string>>::reverse_iterator iter;
            for (iter = m_owordIdMap.rbegin(); iter < m_owordIdMap.rend(); iter++)
            {
                if (iter->first == O_do || iter->first == O_while)
                {
                    break;
                }
            }
            if (iter == m_owordIdMap.rend())
            {
                rtapi_print("###cannot find a matched loop start for break\n");
                ERS(INTERP_ERR_UNMATCHED_BREAK, "unmatched control oword 'break'");
            }
            sprintf(oNameBuf, "%s", iter->second.c_str());
            block->o_type = O_break;
        }
        catch (...)
        {
            ERS(INTERP_ERR_UNMATCHED_BREAK, "unmatched control oword 'break'");
        }
    }
    else if (isToken(line, counter, "continue"))
    {
        try
        {
            std::vector<std::pair<int, std::string>>::reverse_iterator iter;
            for (iter = m_owordIdMap.rbegin(); iter < m_owordIdMap.rend(); iter++)
            {
                if (iter->first == O_do || iter->first == O_while)
                {
                    break;
                }
            }
            if (iter == m_owordIdMap.rend())
            {
                rtapi_print("###cannot find a matched loop start for continue\n");
                ERS(INTERP_ERR_UNMATCHED_CONTINUE, "unmatched control oword 'continue'");
            }
            sprintf(oNameBuf, "%s", iter->second.c_str());
            block->o_type = O_continue;
        }
        catch (...)
        {
            ERS(INTERP_ERR_UNMATCHED_CONTINUE, "unmatched control oword 'continue' ");
        }
    }
    else if (isToken(line, counter, "return"))
    {
        try
        {
            ///FIXME  'return' 与 ‘sub’、‘def'、’call' 的功能需要详细确认。 暂不建议使用这些指令。
            if (m_owordIdMap.size() <= 0)
            {
                ERS(INTERP_ERR_UNMATCHED_RETURN, "unmatched control oword 'return'");
            }
            std::pair<int, std::string> lastItem = m_owordIdMap.back();
            if (lastItem.first != O_sub)
            {
                ERS(INTERP_ERR_UNMATCHED_RETURN, "unmatched control oword 'return'");
            }
            sprintf(oNameBuf, "%s", lastItem.second.c_str());
            block->o_type = O_return;
        }
        catch (...)
        {
            ERS(INTERP_ERR_UNMATCHED_RETURN, "unmatched control oword 'return'");
        }
    }
    else if (isToken(line, counter, "end"))
    {
        if (m_owordIdMap.size())
        {
            std::pair<int, std::string> lastItem;
            if (m_owordIdMap.size() > 0)
                lastItem = m_owordIdMap.back();
            else
            {
                ERS(INTERP_ERR_UNMATCHED_END, "An unmatched 'end' keyword exists in the script");
            }
            // printf("end:  lineNum = %d, lastItem.first = %d ,%s\n", _setup.sequence_number, lastItem.first, lastItem.second.c_str());
            if (lastItem.first == O_while)
            {
                // if (beforeProgStartLine)
                //     lastItem.first = O_none;
                // else
                {
                    sprintf(oNameBuf, "%s", lastItem.second.c_str());
                    // printf("endWhile oNameBuf = %s\n", oNameBuf);
                    block->o_type = O_endwhile;
                    if (m_isSteppingMode == 0)
                    {
                        _waitFlag = 1;
                        enqueue_DWELL(0, 0.001, m_threadId);
                    }
                }
            }
            else if (lastItem.first == O_repeat)
            {
                sprintf(oNameBuf, "%s", lastItem.second.c_str());
                // printf("endRepeat oNameBuf = %s\n", oNameBuf);
                block->o_type = O_endrepeat;
                if (m_isSteppingMode == 0)
                {
                    _waitFlag = 1;
                    enqueue_DWELL(0, 0.001, m_threadId);
                }
            }
            else if (lastItem.first == O_sub)
            {
                sprintf(oNameBuf, "%s", lastItem.second.c_str());
                // printf("endSub oNameBuf = %s\n", oNameBuf);
                block->o_type = O_endsub;
                if (m_isSteppingMode == 0)
                {
                    _waitFlag = 1;
                    enqueue_DWELL(0, 0.001, m_threadId);
                }
            }
            else if (lastItem.first == O_if)
            {
                sprintf(oNameBuf, "%s", lastItem.second.c_str());
                // printf("endIf oNameBuf = %s\n", oNameBuf);
                block->o_type = O_endif;
            }
            else
            {
                ERS(INTERP_ERR_UNMATCHED_END, "An unmatched 'end' keyword exists in the script");
            }
            m_owordIdMap.pop_back();
        }
        else
        {
            rtapi_print("read_o 12 2: unmatched end\n");
            ERS(INTERP_ERR_UNMATCHED_END, "An unmatched 'end' keyword exists in the script");
        }
    }
    else
    {
        block->o_type = O_none;
        return INTERP_NOT_HANDLE;
    }

    switch (block->o_type)
    {
    case O_sub:
    case O_endsub:
    case O_call:
    case O_return:
        block->o_name = strstore(oNameBuf);
        logDebug("global case:|%s|", block->o_name);
        break;
    default:
        if (_setup.call_level)
        {
            subName = _setup.sub_context[_setup.call_level].subName;
            logDebug("inside a call[%d]:|%s|", _setup.call_level, subName);
        }
        else if (_setup.defining_sub)
        {
            subName = _setup.sub_name;
            logDebug("defining_sub:|%s|", subName);
        }
        else
        {
            subName = "";
            logDebug("not defining_sub:|%s|", subName);
        }
        sprintf(fullNameBuf, "%s#%s", subName, oNameBuf);
        block->o_name = strstore(fullNameBuf);
        logDebug("local case:|%s|", block->o_name);
    }
    logDebug("o_type:%s o_name: %s  line:%d %s", o_ops[block->o_type], block->o_name, block->line_number, line);

    if (block->o_type == O_sub)
    {
        block->o_type = O_sub;
    }
    // in terms of execution endsub and return do the same thing
    else if ((block->o_type == O_endsub) || (block->o_type == O_return))
    {
        if ((_setup.skipping_o != 0) && (0 != strcmp(_setup.skipping_o, block->o_name)))
        {
            return INTERP_OK;
        }

        // optional return value expression
        if (line[*counter] == '(')
        {
            ret = read_real_expression(line, counter, &value, parameters);
            CHK_INTERP_READ(ret, line, counter);

            logOword("%s %s value %lf", (block->o_type == O_endsub) ? "endsub" : "return", block->o_name, value);
            _setup.return_value = value;
            _setup.value_returned = 1;
        }
        else
        {
            _setup.return_value = 0;
            _setup.value_returned = 0;
        }
    }
    else if (_setup.defining_sub == 1)
    {
        // we can not evaluate expressions -- so just skip on out
        block->o_type = O_none;
    }
    else if (block->o_type == O_call)
    {
        // we need to NOT evaluate parameters if skipping
        // skipping never ends on a "call"
        if (_setup.skipping_o != 0)
        {
            block->o_type = O_none;
            return INTERP_OK;
        }

        // convey starting state for call_fsm() to handle this call
        // convert_remapped_code() might change this to CS_REMAP
        block->call_type = CT_NGC_OWORD_SUB;

        for (param_cnt = 0; (line[*counter] == '(') || (line[*counter] == '#');)
        {
            if (line[*counter] == '#')
            {
                CHP(read_comment(line, counter, block, parameters));
                continue;
            }
            logDebug("counter[%d] rest of line:|%s|", *counter, line + *counter);
            CHKS((param_cnt >= INTERP_SUB_PARAMS), INTERP_ERR_SYNTAX_FAILED, NCE_TOO_MANY_SUBROUTINE_PARAMETERS);

            ret = read_real_expression(line, counter, &value, parameters);
            CHK_INTERP_READ(ret, line, counter);

            block->params[param_cnt] = value;
            param_cnt++;
        }
        logDebug("set arg params:%d", param_cnt);
        block->param_cnt = param_cnt;

        // zero the remaining params
        for (; param_cnt < INTERP_SUB_PARAMS; param_cnt++) { block->params[param_cnt] = 0.0; }
    }
    else if (block->o_type == O_do)
    {
        block->o_type = O_do;
    }
    else if (block->o_type == O_while)
    {
        // TESTME !!!KL -- should not eval expressions if skipping ???
        if ((_setup.skipping_o != 0) && (0 != strcmp(_setup.skipping_o, block->o_name)))
        {
            return INTERP_OK;
        }

        block->o_type = O_while;
        CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, _("Left bracket missing after 'while'"));

        ret = read_real_expression(line, counter, &value, parameters);
        CHK_INTERP_READ(ret, line, counter);
        _setup.test_value = value;
        // skip the ':' character
        *counter = strlen(line);
    }
    else if (block->o_type == O_repeat)
    {
        if ((_setup.skipping_o != 0) && (0 != strcmp(_setup.skipping_o, block->o_name)))
        {
            return INTERP_OK;
        }

        block->o_type = O_repeat;
        CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, _("Left bracket missing after 'repeat'"));

        ret = read_real_expression(line, counter, &value, parameters);
        CHK_INTERP_READ(ret, line, counter);
        _setup.test_value = value;
        // skip the ':' character
        *counter = strlen(line);
    }
    else if (block->o_type == O_if)
    {
        if ((_setup.skipping_o != 0) && (0 != strcmp(_setup.skipping_o, block->o_name)))
        {
            return INTERP_OK;
        }

        block->o_type = O_if;
        CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, _("Left bracket missing after 'if'"));

        ret = read_real_expression(line, counter, &value, parameters);
        CHK_INTERP_READ(ret, line, counter);
        _setup.test_value = value;
        // skip the ':' character
        *counter = strlen(line);
    }
    else if (block->o_type == O_elif)
    {
        if ((_setup.skipping_o != 0) && (0 != strcmp(_setup.skipping_o, block->o_name)))
        {
            return INTERP_OK;
        }
        block->o_type = O_elif;
        CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, _("Left bracket missing after 'elseif'"));
        ret = read_real_expression(line, counter, &value, parameters);
        CHK_INTERP_READ(ret, line, counter);
        _setup.test_value = value;
        // skip the ':' character
        *counter = strlen(line);
    }
    else if (block->o_type == O_else)
    {
        block->o_type = O_else;
        // skip the ':' character
        *counter = strlen(line);
    }
    else if (block->o_type == O_endif)
    {
        block->o_type = O_endif;
    }
    else if (block->o_type == O_break)
    {
        block->o_type = O_break;
    }
    else if (block->o_type == O_continue)
    {
        block->o_type = O_continue;
    }
    else if (block->o_type == O_endwhile)
    {
        block->o_type = O_endwhile;
    }
    else if (block->o_type == O_endrepeat)
    {
        block->o_type = O_endrepeat;
    }
    else
    {
        // not legal
        block->o_type = O_none;
        // ERS(INTERP_ERR_UNKNOWN_COMMAND_IN_O_LINE, NCE_UNKNOWN_COMMAND_IN_O_LINE);
    }

    return INTERP_OK;
}

/****************************************************************************/

/*! read_p

 Returned Value: int
 If read_real_value returns an error code, this returns that code.
 If any of the following errors occur, this returns the error code shown.
 Otherwise, it returns INTERP_OK.
 1. The first character read is not p:
 NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED
 2. A p value has already been inserted in the block:
 NCE_MULTIPLE_P_WORDS_ON_ONE_LINE

 Side effects:
 counter is reset to point to the first character following the p value.
 The p value setting is inserted in block.

 Called by: read_one_item

 When this function is called, counter is pointing at an item on the
 line that starts with the character 'p', indicating a p value
 setting. The function reads characters which tell how to set the p
 value, up to the start of the next item or the end of the line. This
 information is inserted in the block.

 P codes are used for:
 1. Dwell time in canned cycles g82, G86, G88, G89 [NCMS pages 98 - 100].
 2. A key with G10 [NCMS, pages 9, 10].

 */

int Interp::read_p(char* line,           //!< string: line of RS274/NGC code being processed
                   int* counter,         //!< pointer to a counter for position on the line
                   block_pointer block,  //!< pointer to a block being filled from the line
                   double* parameters)   //!< array of system parameters
{
    jaka_value value;

    CHKS((line[*counter] != 'p'), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = (*counter + 1);
    CHKS((block->p_number > -1.0), INTERP_ERR_SYNTAX_FAILED, NCE_MULTIPLE_P_WORDS_ON_ONE_LINE);
    CHP(read_real_value(line, counter, &value, parameters));
    // FMP removed check for negatives, since we may want them for
    // user-defined codes
    // CHKS((value < 0.0), INTERP_ERR_SYNTAX_FAILED, NCE_NEGATIVE_P_WORD_USED);
    block->p_number = value.scalarValue;
    block->p_flag = true;
    return INTERP_OK;
}

int Interp::read_name(char* line,     //!< string: line of RS274/NGC code being processed
                      int* counter,   //!< pointer to a counter for position on the line
                      char* nameBuf)  //!< pointer to name to be read
{
    int done = 0;
    int i;

    // rtapi_print("read_name: line is %s, *counter=%d\n", line, *counter);

    // skip the spaces
    // read the alphabet name until none alphabetic character
    std::set<char> separatorList{' ', '=', ',', '+', '-', '*', '/', '^', '(', '[', '{', ')', ']', '}', '!', '<', '>', '%', '&', '|', ':'};
    for (i = 0; (i < LINELEN) && (*counter < (int)strlen(line)); i++)
    {
        if (separatorList.count(line[*counter]))
        {                       // the end of a varible name
            nameBuf[i] = '\0';  // terminate the name
            done = 1;
            break;
        }
        nameBuf[i] = line[*counter];
        *counter = (*counter + 1);
    }

    if (*counter == (int)strlen(line) && done == 0)
    {
        done = 1;
        nameBuf[i] = '\0';
    }
    // printf("line is: %s, length = %d, i = %d, done = %d, name is %s\n", line, strlen(line), i, done, nameBuf);
    // !!!KL need to rename the error message and change text
    CHKS((!done), INTERP_ERR_SYNTAX_FAILED, NCE_NAMED_PARAMETER_NOT_TERMINATED);

    return INTERP_OK;
}

/****************************************************************************/

/*! read_parameter

 Returned Value: int
 If read_integer_value returns an error code, this returns that code.
 If any of the following errors occur, this returns the error code shown.
 Otherwise, this returns INTERP_OK.
 1. The first character read is not # :
 NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED
 2. The parameter number is out of bounds:
 NCE_PARAMETER_NUMBER_OUT_OF_RANGE

 Side effects:
 The value of the given parameter is put into what double_ptr points at.
 The counter is reset to point to the first character after the
 characters which make up the value.

 Called by:  read_real_value

 This attempts to read the value of a parameter out of the line,
 starting at the index given by the counter.

 According to the RS274/NGC manual [NCMS, p. 62], the characters following
 # may be any "parameter expression". Thus, the following are legal
 and mean the same thing (the value of the parameter whose number is
 stored in parameter 2):
 ##2
 #[#2]

 Parameter setting is done in parallel, not sequentially. For example
 if #1 is 5 before the line "#1=10 #2=#1" is read, then after the line
 is is executed, #1 is 10 and #2 is 5. If parameter setting were done
 sequentially, the value of #2 would be 10 after the line was executed.

 ADDED by K. Lerman
 Named parameters are now supported.
 #[abcd] is a parameter with name "abcd"
 #[#2] is NOT a named parameter.
 When a [ is seen after a #, if the next char is not a #, it is a named
 parameter.

 */

int Interp::read_parameter(char* line,             //!< string: line of RS274/NGC code being processed
                           int* counter,           //!< pointer to a counter for position on the line
                           jaka_value* value_ptr,  //!< pointer to double to be read
                           double* parameters,     //!< array of system parameters
                           bool check_exists)      //!< test for existence, not value
{
    int strtIdx;
    // static char name[] = "read_named_parameter";
    char paramNameBuf[LINELEN + 1];

    // rtapi_print("before reading name: %s, counter=%d\n", line, *counter);
    CHKS(((line[*counter] < 'a' || line[*counter] > 'z') && line[*counter] != '_'), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    CHP(read_name(line, counter, paramNameBuf));
    // rtapi_print("result of read_name: %s\n", paramNameBuf);

    // reserved key word
    if (strcmp(paramNameBuf, "sysvar") != 0)
    {
        CHP(read_named_parameter(line, counter, paramNameBuf, value_ptr, parameters, check_exists));
        if (line[*counter] == '[')
        {
            *counter += 1;
            int ret = read_integer_value(line, counter, &strtIdx, parameters);
            CHK_INTERP_READ(ret, line, counter);
            bool endIdxFlag = false;
            int endIdx, step = 1;
            if (line[*counter] == ':')
            {  // end
                *counter += 1;
                endIdxFlag = true;
                ret = read_integer_value(line, counter, &endIdx, parameters);
                // rtapi_print("array access: endindex = %d\n", endIdx);
                CHK_INTERP_READ(ret, line, counter);
                if (line[*counter] == ':')
                {  // step
                    *counter += 1;
                    ret = read_integer_value(line, counter, &step, parameters);
                    CHK_INTERP_READ(ret, line, counter);
                }
                // rtapi_print("array access: step = %d\n", step);
            }
            CHKS((line[*counter] != ']'), INTERP_ERR_SYNTAX_FAILED, "']' should be used to access an array variable");
            if (value_ptr != nullptr)
            {
                if (value_ptr->valueType != ARRAY_VALUE)
                {
                    ERS(INTERP_ERR_INVALID_ARRAY, "the variable is not an array element");
                }
                int arrSize = value_ptr->arrValue.size();
                if (endIdxFlag)
                {
                    jaka_value subArray;
                    subArray.valueType = ARRAY_VALUE;
                    subArray.arrValue.clear();
                    // rtapi_print("endIdxFlag = %d, step = %d\n", endIdxFlag, step);
                    if (step == 0)
                    {
                        ERS(INTERP_ERR_INVALID_ARRAY_SLICE_STEP, "ValueError: slice step cannot be zero");
                    }
                    else if (abs(strtIdx) >= arrSize || abs(endIdx) > arrSize)
                    {
                        ERS(INTERP_ERR_INVALID_INDEX, "invalid array element index");
                    }
                    else
                    {
                        strtIdx = (strtIdx <= (arrSize - 1) ? strtIdx : (arrSize - 1));
                        strtIdx = (strtIdx >= (-1 * arrSize) ? strtIdx : (-1 * arrSize));
                        endIdx = (endIdx <= arrSize ? endIdx : arrSize);
                        endIdx = (endIdx >= (-1 * arrSize - 1) ? endIdx : (-1 * arrSize - 1));
                        // rtapi_print("9: strtIdx = %d, endIdx = %d, step = %d, arraySize = %d\n", strtIdx, endIdx, step, value_ptr->arrValue.size());
                        if (step > 0 && strtIdx < endIdx)
                        {
                            if (strtIdx < 0 && endIdx >= 0)
                            {
                                strtIdx = value_ptr->arrValue.size() + strtIdx;
                            }
                            // rtapi_print("1: strtIdx = %d, endIdx = %d, step = %d\n", strtIdx, endIdx, step);
                            for (int j = strtIdx; j < endIdx; j = j + step)
                            {
                                int realIdx = (j >= 0 ? j : (arrSize + j));
                                subArray.arrValue.push_back(value_ptr->arrValue[realIdx]);
                                // rtapi_print("element append: index = %d, value = %f\n", realIdx, value_ptr->arrValue[realIdx]);
                            }
                        }
                        else if (step < 0 && strtIdx > endIdx)
                        {
                            if (strtIdx >= 0 && endIdx < 0)
                            {
                                endIdx = value_ptr->arrValue.size() + endIdx;
                            }
                            for (int j = strtIdx; j > endIdx; j = j + step)
                            {
                                int realIdx = (j >= 0 ? j : (arrSize + j));
                                subArray.arrValue.push_back(value_ptr->arrValue[realIdx]);
                                // rtapi_print("element append: index = %d, value = %f\n", realIdx, value_ptr->arrValue[realIdx]);
                            }
                        }
                    }
                    // rtapi_print("sub array size = %d\n", subArray.arrValue.size());
                    value_ptr->valueType = ARRAY_VALUE;
                    value_ptr->arrValue.clear();
                    for (size_t j = 0; j < subArray.arrValue.size(); j++) { value_ptr->arrValue.push_back(subArray.arrValue[j]); }
                }
                else
                {
                    value_ptr->valueType = SCALAR_VALUE;
                    if (strtIdx >= arrSize || (strtIdx < -1 * arrSize))
                    {
                        ERS(INTERP_ERR_INVALID_INDEX, "invalid array element index");
                    }
                    else if (strtIdx >= 0 && strtIdx < arrSize)
                    {
                        value_ptr->scalarValue = value_ptr->arrValue[strtIdx];
                        // rtapi_print("positive index: element value = %f\n", value_ptr->arrValue[arrSize+strtIdx]);
                    }
                    else if (strtIdx < 0 && strtIdx >= (-1 * arrSize))
                    {
                        value_ptr->scalarValue = value_ptr->arrValue[arrSize + strtIdx];
                        // rtapi_print("negative index: element value = %f\n", value_ptr->arrValue[arrSize+strtIdx]);
                    }
                    // rtapi_print("array element value = %f\n", value_ptr->scalarValue);
                }
            }
            *counter += 1;  // skip current char
        }
    }
    else
    {
        CHKS((line[*counter] != '['), INTERP_ERR_SYNTAX_FAILED, "'[' should be used to access a system variable");
        *counter += 1;
        int ret = read_integer_value(line, counter, &strtIdx, parameters);
        CHK_INTERP_READ(ret, line, counter);
        CHKS((line[*counter] != ']'), INTERP_ERR_SYNTAX_FAILED, "']' should be used to access a system variable");
        *counter += 1;  // skip current char

        if (check_exists)
        {
            value_ptr->valueType = SCALAR_VALUE;
            value_ptr->scalarValue = (strtIdx >= 1 && strtIdx < RS274NGC_MAX_PARAMETERS);
            return INTERP_OK;
        }
        CHKS(((strtIdx < 1) || (strtIdx >= RS274NGC_MAX_PARAMETERS)), INTERP_ERR_SYNTAX_FAILED, NCE_PARAMETER_NUMBER_OUT_OF_RANGE);
        CHKS(((strtIdx >= 5420) && (strtIdx <= 5428) && (_setup.cutter_comp_side)),
             INTERP_ERR_SYNTAX_FAILED,
             _("Cannot read current position with cutter radius compensation on"));

        value_ptr->valueType = SCALAR_VALUE;
        value_ptr->scalarValue = parameters[strtIdx];
    }
    return INTERP_OK;
}

int Interp::read_bracketed_parameter(char* line,             //!< string: line of RS274/NGC code being processed
                                     int* counter,           //!< pointer to a counter for position on the line
                                     jaka_value* value_ptr,  //!< pointer to double to be read
                                     double* parameters,     //!< array of system parameters
                                     bool check_exists)      //!< test for existence, not value
{
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = (*counter + 1);
    CHKS(line[*counter] != '_' && (line[*counter] < 'a' || line[*counter] > 'z'), INTERP_ERR_SYNTAX_FAILED, _("Expected # reading parameter"));
    CHP(read_parameter(line, counter, value_ptr, parameters, check_exists));
    CHKS((line[*counter] != ')'), INTERP_ERR_SYNTAX_FAILED, _("Expected ] reading bracketed parameter"));
    *counter = (*counter + 1);
    return INTERP_OK;
}

/****************************************************************************/

/*@brief function to read parameter setting

 Returned Value: int
 If read_real_value or read_integer_value returns an error code,
 this returns that code.
 If any of the following errors occur, this returns the error code shown.
 Otherwise, it returns INTERP_OK.
 1. The first character read is not # :
 NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED
 2. The parameter index is out of range: PARAMETER_NUMBER_OUT_OF_RANGE
 3. An equal sign does not follow the parameter expression:
 NCE_EQUAL_SIGN_MISSING_IN_PARAMETER_SETTING

 Side effects:
 counter is reset to the character following the end of the parameter
 setting. The parameter whose index follows "#" is set to the
 real value following "=".

 Called by: read_one_item

 When this function is called, counter is pointing at an item on the
 line that starts with the character '#', indicating a parameter
 setting when found by read_one_item.  The function reads characters
 which tell how to set the parameter.

 Any number of parameters may be set on a line. If parameters set early
 on the line are used in expressions farther down the line, the
 parameters have their old values, not their new values. This is
 usually called setting parameters in parallel.

 Parameter setting is not clearly described in [NCMS, pp. 51 - 62]: it is
 not clear if more than one parameter setting per line is allowed (any
 number is OK in this implementation). The characters immediately following
 the "#" must constitute a "parameter expression", but it is not clear
 what that is. Here we allow any expression as long as it evaluates to
 an integer.

 Parameters are handled in the interpreter by having a parameter table
 and a parameter buffer as part of the machine settings. The parameter
 table is passed to the reading functions which need it. The parameter
 buffer is used directly by functions that need it. Reading functions
 may set parameter values in the parameter buffer. Reading functions
 may obtain parameter values; these come from parameter table.

 The parameter buffer has three parts: (i) a counter for how many
 parameters have been set while reading the current line (ii) an array
 of the indexes of parameters that have been set while reading the
 current line, and (iii) an array of the values for the parameters that
 have been set while reading the current line; the nth value
 corresponds to the nth index. Any given index will appear once in the
 index number array for each time the parameter with that index is set
 on a line. There is no point in setting the same parameter more than
 one on a line because only the last setting of that parameter will
 take effect.

 The syntax recognized by this this function is # followed by an
 integer expression (explicit integer or expression evaluating to an
 integer) followed by = followed by a real value (number or
 expression).

 Note that # also starts a bunch of characters which represent a parameter
 to be evaluated. That situation is handled by read_parameter.

 */
std::mutex sys_sources;

int Interp::read_parameter_setting(char* line,           //!< string: line of RS274/NGC code being processed
                                   int* counter,         //!< pointer to a counter for position on the line
                                   block_pointer block,  //!< pointer to a block being filled from the line
                                   double* parameters)   //!< array of system parameters
{
    std::unique_lock<std::mutex> m_mutex(sys_sources, std::defer_lock);
    if (threadMutexState())
        m_mutex.lock();
    static char name[] = "read_parameter_setting";
    int index;
    jaka_value value;
    char* param;
    const char* dup;
    int ret;

    if (isToken(line, counter, "sysvar") == 0)
    {
        /*named parameters look like 'letter...' or '_letter.....'*/
        if (line[*(counter)] == '_' || (line[*counter] >= 'a' && line[*counter] <= 'z'))
        {
            int arrEleIndex = 0;
            bool bArrElement = false;
            jaka_value arrValue;
            CHP(read_named_parameter_setting(line, counter, &param, parameters));  ///< added the param to the map and set the default value
            // rtapi_print("read_parameter_setting, line[%d] = %c, param = %s\n", *(counter), line[*(counter)], param);
            // check if it's an array element assignment
            if (line[*counter] == '[')
            {
                *counter += 1;
                // rtapi_print("before read_real_value: line = %s\n", line);
                ret = read_real_value(line, counter, &value, parameters);
                CHK_INTERP_READ(ret, line, counter);
                CHKS((line[*counter] != ']'), INTERP_ERR_SYNTAX_FAILED, "']' should be used to access an array variable");
                bArrElement = true;
                arrEleIndex = round_to_int(value.scalarValue);
                *counter = (*counter + 1);

                // get the old value
                int exists;
                CHP(find_named_param(param, &exists, &arrValue));
                if (!exists)
                {
                    ERS(INTERP_ERR_INVALID_ARRAY, "the array %s doesn't exist", param);
                }
                else if (arrValue.valueType != ARRAY_VALUE)
                {
                    ERS(INTERP_ERR_INVALID_ARRAY, "%s is not an array varible", param);
                }
                else
                {
                    int arrSize = arrValue.arrValue.size();
                    if (arrEleIndex < -1 * arrSize || arrEleIndex >= arrSize)
                    {
                        ERS(INTERP_ERR_INVALID_INDEX, "invalid array element index");
                    }
                }
            }

            CHKS((line[*counter] != '='), INTERP_ERR_SYNTAX_FAILED, NCE_EQUAL_SIGN_MISSING_IN_PARAMETER_SETTING);
            *counter = (*counter) + 1;  // skip the '='

            if (line[*counter] == '[')
            {
                if (!bArrElement)
                {
                    ret = read_array_value(line, counter, &value, parameters);
                    *counter = strlen(line);
                    CHK_INTERP_READ(ret, line, counter);
                }
                else
                {
                    ERS(INTERP_ERR_ARRAY_NEST_UNSUPPORT, "array cannot be nested now");
                }
            }
            else if (line[*counter] == '\"')
            {
                if (!bArrElement)
                {
                    ret = read_string_value(line, counter, &value, parameters);
                    *counter = strlen(line);
                    CHK_INTERP_READ(ret, line, counter);
                }
                else
                {
                    ERS(INTERP_ERR_STRING_ARRAY_UNSUPPORT, "string array is now unsupported");
                }
            }
            else
            {
                ret = read_real_value(line, counter, &value, parameters);
                CHK_INTERP_READ(ret, line, counter);
                if (bArrElement)
                {
                    int arrSize = arrValue.arrValue.size();
                    arrEleIndex = ((arrEleIndex >= 0) ? arrEleIndex : (arrEleIndex + arrSize));
                    arrValue.arrValue[arrEleIndex] = value.scalarValue;
                }
                logDebug("setting up named param[%d]:|%s| value:%lf", _setup.named_parameter_occurrence, param, value.scalarValue);
            }

            dup = strstore(param);  // no more need to free this
            if (dup == 0)
            {
                ERS(INTERP_ERR_OUT_OF_MEMORY, NCE_OUT_OF_MEMORY);
            }

            logDebug("%s |%s|", name, dup);
            _setup.named_parameters[_setup.named_parameter_occurrence] = dup;
            _setup.named_parameter_values[_setup.named_parameter_occurrence] = bArrElement ? arrValue : value;
            _setup.named_parameter_occurrence++;
            // rtapi_print("done setting up named param[%d]:|%s| value:%lf\n", _setup.named_parameter_occurrence, param, value.scalarValue);
        }
    }
    else
    {  // set global parameter
        // synch parsing and execution
        SYNCH_PARSE_EXEC;
        CHKS((line[*counter] != '['), INTERP_ERR_SYNTAX_FAILED, "'[' should be used to access a system variable");
        *counter += 1;
        CHP(read_integer_value(line, counter, &index, parameters));
        CHKS((line[*counter] != ']'), INTERP_ERR_SYNTAX_FAILED, "']' should be used to access a system variable");
        *counter += 1;
        CHKS(((index < 1) || (index >= RS274NGC_MAX_PARAMETERS)), INTERP_ERR_SYNTAX_FAILED, NCE_PARAMETER_NUMBER_OUT_OF_RANGE);
        CHKS((isreadonly(index)), INTERP_ERR_SYNTAX_FAILED, NCE_PARAMETER_NUMBER_READONLY);
        CHKS((line[*counter] != '='), INTERP_ERR_SYNTAX_FAILED, NCE_EQUAL_SIGN_MISSING_IN_PARAMETER_SETTING);
        *counter = (*counter + 1);

        ret = read_real_value(line, counter, &value, parameters);
        CHK_INTERP_READ(ret, line, counter);

        _setup.parameter_numbers[_setup.parameter_occurrence] = index;
        _setup.parameter_values[_setup.parameter_occurrence] = value.scalarValue;
        _setup.parameter_occurrence++;
        // rtapi_print("$$$$$$$$$$$$$$$ write sysval[%d] = %f\n", index, value.scalarValue);
    }
    if (threadMutexState())
    {
        m_mutex.unlock();
    }
    return INTERP_OK;
}

/****************************************************************************/

/*! read_named_parameter_setting

 Returned Value: int
 If read_real_value or read_integer_value returns an error code,
 this returns that code.
 If any of the following errors occur, this returns the error code shown.
 Otherwise, it returns INTERP_OK.
 1. The first character read is not # :
 NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED
 2. The parameter index is out of range: PARAMETER_NUMBER_OUT_OF_RANGE
 3. An equal sign does not follow the parameter expression:
 NCE_EQUAL_SIGN_MISSING_IN_PARAMETER_SETTING

 Side effects:
 counter is reset to the character following the end of the parameter
 setting. The parameter whose index follows "#" is set to the
 real value following "=".

 Called by: read_parameter_setting

 When this function is called, counter is pointing at an item on the
 line that starts with the character '#', indicating a parameter
 setting when found by read_one_item.  The function reads characters
 which tell how to set the parameter.

 Any number of parameters may be set on a line. If parameters set early
 on the line are used in expressions farther down the line, the
 parameters have their old values, not their new values. This is
 usually called setting parameters in parallel.

 Parameter setting is not clearly described in [NCMS, pp. 51 - 62]: it is
 not clear if more than one parameter setting per line is allowed (any
 number is OK in this implementation). The characters immediately following
 the "#" must constitute a "parameter expression", but it is not clear
 what that is. Here we allow any expression as long as it evaluates to
 an integer.

 Parameters are handled in the interpreter by having a parameter table
 and a parameter buffer as part of the machine settings. The parameter
 table is passed to the reading functions which need it. The parameter
 buffer is used directly by functions that need it. Reading functions
 may set parameter values in the parameter buffer. Reading functions
 may obtain parameter values; these come from parameter table.

 The parameter buffer has three parts: (i) a counter for how many
 parameters have been set while reading the current line (ii) an array
 of the indexes of parameters that have been set while reading the
 current line, and (iii) an array of the values for the parameters that
 have been set while reading the current line; the nth value
 corresponds to the nth index. Any given index will appear once in the
 index number array for each time the parameter with that index is set
 on a line. There is no point in setting the same parameter more than
 one on a line because only the last setting of that parameter will
 take effect.

 The syntax recognized by this this function is # followed by an
 integer expression (explicit integer or expression evaluating to an
 integer) followed by = followed by a real value (number or
 expression).

 Note that # also starts a bunch of characters which represent a parameter
 to be evaluated. That situation is handled by read_parameter.

 */

int Interp::read_named_parameter_setting(char* line,          //!< string: line of RS274/NGC code being processed
                                         int* counter,        //!< pointer to a counter for position on the line
                                         char** param,        //!< pointer to the char * to be returned
                                         double* parameters)  //!< array of system parameters
{
    static char name[] = "read_named_parameter_setting";
    int status;
    static char paramNameBuf[LINELEN + 1];

    *param = paramNameBuf;

    logDebug("entered %s", name);

    status = read_name(line, counter, paramNameBuf);
    CHP(status);

    // rtapi_print("%s: returned(%d) from read_name:|%s|\n", name, status, paramNameBuf);

    status = add_named_param(paramNameBuf);
    CHP(status);
    logDebug("%s: returned(%d) from add_named_param:|%s|", name, status, paramNameBuf);

    // the rest of the work is done in read_parameter_setting

    return INTERP_OK;
}

/****************************************************************************/

/*! read_q

 Returned Value: int
 If read_real_value returns an error code, this returns that code.
 If any of the following errors occur, this returns the error code shown.
 Otherwise, it returns INTERP_OK.
 1. The first character read is not q:
 NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED
 2. A q value has already been inserted in the block:
 NCE_MULTIPLE_Q_WORDS_ON_ONE_LINE

 Side effects:
 counter is reset to point to the first character following the q value.
 The q value setting is inserted in block.

 Called by: read_one_item

 When this function is called, counter is pointing at an item on the
 line that starts with the character 'q', indicating a q value
 setting. The function reads characters which tell how to set the q
 value, up to the start of the next item or the end of the line. This
 information is inserted in the block.

 Q is used only in the G87 canned cycle [NCMS, page 98], where it must
 be positive.

 */

int Interp::read_q(char* line,           //!< string: line of RS274/NGC code being processed
                   int* counter,         //!< pointer to a counter for position on the line
                   block_pointer block,  //!< pointer to a block being filled from the line
                   double* parameters)   //!< array of system parameters
{
    jaka_value value;

    CHKS((line[*counter] != 'q'), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = (*counter + 1);
    CHKS((block->q_number > -1.0), INTERP_ERR_SYNTAX_FAILED, NCE_MULTIPLE_Q_WORDS_ON_ONE_LINE);
    CHP(read_real_value(line, counter, &value, parameters));
    // FMP removed check for negatives, since we may want them for
    // user-defined codes
    // CHKS((value <= 0.0), INTERP_ERR_SYNTAX_FAILED, NCE_NEGATIVE_OR_ZERO_Q_VALUE_USED);
    block->q_number = value.scalarValue;
    block->q_flag = true;
    return INTERP_OK;
}

/****************************************************************************/

/*! read_r

 Returned Value: int
 If read_real_value returns an error code, this returns that code.
 If any of the following errors occur, this returns the error code shown.
 Otherwise, it returns INTERP_OK.
 1. The first character read is not r:
 NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED
 2. An r_number has already been inserted in the block:
 NCE_MULTIPLE_R_WORDS_ON_ONE_LINE

 Side effects:
 counter is reset.
 The r_flag in the block is turned on.
 The r_number is inserted in the block.

 Called by: read_one_item

 When this function is called, counter is pointing at an item on the
 line that starts with the character 'r'. The function reads characters
 which tell how to set the coordinate, up to the start of the next item
 or the end of the line. This information is inserted in the block. The
 counter is then set to point to the character following.

 An r number indicates the clearance plane in canned cycles.
 An r number may also be the radius of an arc.

 The value may be a real number or something that evaluates to a
 real number, so read_real_value is used to read it. Parameters
 may be involved.

 */

int Interp::read_r(char* line,           //!< string: line of RS274 code being processed
                   int* counter,         //!< pointer to a counter for position on the line
                   block_pointer block,  //!< pointer to a block being filled from the line
                   double* parameters)   //!< array of system parameters
{
    jaka_value value;

    CHKS((line[*counter] != 'r'), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = (*counter + 1);
    CHKS((block->r_flag), INTERP_ERR_SYNTAX_FAILED, NCE_MULTIPLE_R_WORDS_ON_ONE_LINE);
    CHP(read_real_value(line, counter, &value, parameters));
    block->r_flag = true;
    block->r_number = value.scalarValue;
    return INTERP_OK;
}

/****************************************************************************/

/*! read_real_expression

 Returned Value: int
 If any of the following functions returns an error code,
 this returns that code.
 read_real_value
 read_operation
 execute_binary
 If any of the following errors occur, this returns the error shown.
 Otherwise, it returns INTERP_OK.
 1. The first character is not [ :
 NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED

 Side effects:
 The number read from the line is put into what value_ptr points at.
 The counter is reset to point to the first character after the real
 expression.

 Called by:
 read_atan
 read_real_value
 read_unary

 Example 1: [2 - 3 * 4 / 5] means [2 - [[3 * 4] / 5]] and equals -0.4.

 Segmenting Expressions -

 The RS274/NGC manual, section 3.5.1.1 [NCMS, page 50], provides for
 using square brackets to segment expressions.

 Binary Operations -

 The RS274/NGC manual, section 3.5.1.1, discusses expression evaluation.
 The manual provides for eight binary operations: the four basic
 mathematical operations (addition, subtraction, multiplication,
 division), three logical operations (non-exclusive ||, exclusive ||,
 and AND2) and the modulus operation. The manual does not explicitly call
 these "binary" operations, but implicitly recognizes that they are
 binary. We have added the "power" operation of raising the number
 on the left of the operation to the power on the right; this is
 needed for many basic machining calculations.

 There are two groups of binary operations given in the manual. If
 operations are strung together as shown in Example 1, operations in
 the first group are to be performed before operations in the second
 group. If an expression contains more than one operation from the same
 group (such as * and / in Example 1), the operation on the left is
 performed first. The first group is: multiplication (*), division (/),
 and modulus (MOD). The second group is: addition(+), subtraction (-),
 logical non-exclusive || (||), logical exclusive || (XOR), and logical
 && (AND2). We have added a third group with higher precedence than
 the first group. The third group contains only the power (**)
 operation.

 The logical operations and modulus are apparently to be performed on
 any real numbers, not just on integers or on some other data type.

 Unary Operations -

 The RS274/NGC manual, section 3.5.1.2, provides for fifteen unary
 mathematical operations. Two of these, BIN and BCD, are apparently for
 converting between decimal and hexadecimal number representation,
 although the text is not clear. These have not been implemented, since
 we are not using any hexadecimal numbers. The other thirteen unary
 operations have been implemented: absolute_value, arc_cosine, arc_sine,
 arc_tangent, cosine, e_raised_to, fix_down, fix_up, natural_log_of,
 round, sine, square_root, tangent.

 The manual section 3.5.1.2 [NCMS, page 51] requires the argument to
 all unary operations (except atan) to be in square brackets.  Thus,
 for example "sin[90]" is allowed in the interpreter, but "sin 90" is
 not. The atan operation must be in the format "atan[..]/[..]".

 Production Rule Definitions in Terms of Tokens -

 The following is a production rule definition of what this RS274NGC
 interpreter recognizes as valid combinations of symbols which form a
 recognized real_value (the top of this production hierarchy).

 The notion of "integer_value" is used in the interpreter. Below it is
 defined as a synonym for real_value, but in fact a constraint is added
 which cannot be readily written in a production language.  An
 integer_value is a real_value which is very close to an integer.
 Integer_values are needed for array and table indices and (when
 divided by 10) for the values of M codes and G codes. All numbers
 (including integers) are read as real numbers and stored as doubles.
 If an integer_value is required in some situation, a test for being
 close to an integer is applied to the number after it is read.


 arc_tangent_combo = arc_tangent expression divided_by expression .

 binary_operation1 = divided_by | modulo | power | times .

 binary_operation2 = and | exclusive_or | minus |  non_exclusive_or | plus .

 combo1 = real_value { binary_operation1 real_value } .

 digit = zero | one | two | three | four | five | six | seven |eight | nine .

 expression =
 left_bracket
 (unary_combo | (combo1 { binary_operation2 combo1 }))
 right_bracket .

 integer_value = real_value .

 ordinary_unary_combo =  ordinary_unary_operation expression .

 ordinary_unary_operation =
 absolute_value | arc_cosine | arc_sine | cosine | e_raised_to |
 fix_down | fix_up | natural_log_of | round | sine | square_root | tangent .

 parameter_index = integer_value .

 parameter_value = parameter_sign  parameter_index .

 real_number =
 [ plus | minus ]
 (( digit { digit } decimal_point {digit}) | ( decimal_point digit {digit})).

 real_value =
 real_number | expression | parameter_value | unary_combo.

 unary_combo = ordinary_unary_combo | arc_tangent_combo .


 Production Tokens in Terms of Characters -

 absolute_value   = 'abs'
 and              = 'and'
 arc_cosine       = 'acos'
 arc_sine         = 'asin'
 arc_tangent      = 'atan'
 cosine           = 'cos'
 decimal_point    = '.'
 divided_by       = '/'
 eight            = '8'
 exclusive_or     = 'xor'
 e_raised_to      = 'exp'
 five             = '5'
 fix_down         = 'fix'
 fix_up           = 'fup'
 four             = '4'
 left_bracket     = '('
 minus            = '-'
 modulo           = 'mod'
 natural_log_of   = 'ln'
 nine             = '9'
 non_exclusive_or = 'or'
 one              = '1'
 parameter_sign   = '#'
 plus             = '+'
 power            = '**'
 right_bracket    = ')'
 round            = 'round'
 seven            = '7'
 sine             = 'sin'
 six              = '6'
 square_root      = 'sqrt'
 tangent          = 'tan'
 three            = '3'
 times            = '*'
 two              = '2'
 zero             = '0'

 When this function is called, the counter should be set at a left
 bracket. The function reads up to and including the right bracket
 which closes the expression.

 The basic form of an expression is: [v1 bop v2 bop ... vn], where the
 vi are real_values and the bops are binary operations. The vi may be
 numbers, parameters, expressions, or unary functions. Because some
 bops are to be evaluated before others, for understanding the order of
 evaluation, it is useful to rewrite the general form collecting any
 subsequences of bops of the same precedence. For example, suppose the
 expression is: [9+8*7/6+5-4*3**2+1]. It may be rewritten as:
 [9+[8*7/6]+5-[4*[3**2]]+1] to show how it should be evaluated.

 The manual provides that operations of the same precedence should be
 processed left to right.

 The first version of this function is commented out. It is suitable
 for when there are only two precendence levels. It is an improvement
 over the version used in interpreters before 2000, but not as general
 as the second version given here.

 The first version of this function reads the first value and the first
 operation in the expression. Then it calls either read_rest_bop1 or
 read_rest_bop2 according to whether the first operation is a bop1 or a
 bop2.  Read_rest_bop1 resets the next_operation to either a right
 bracket or a bop2. If it is reset to a bop2, read_rest_bop2 is called
 when read_rest_bop1 returns.

 */

#ifdef UNDEFINED
int Interp::read_real_expression(char* line,          //!< string: line of RS274/NGC code being processed
                                 int* counter,        //!< pointer to a counter for position on the line
                                 double* value,       //!< pointer to double to be read
                                 double* parameters)  //!< array of system parameters
{
    static char name[] = "read_real_expression";
    int next_operation;
    int status;
    int ret;

    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = (*counter + 1);

    ret = read_real_value(line, counter, value, parameters);
    CHK_INTERP_READ(ret, line, counter);

    CHP(read_operation(line, counter, &next_operation));
    if (next_operation == RIGHT_BRACKET || next_operation == RIGHT_SBRACKET || next_operation == RIGHT_COMMA)
        ; /* nothing to do */
    else if (next_operation < AND2)
    { /* next operation is a bop1, times-like */
        CHP(read_rest_bop1(line, counter, value, &next_operation, parameters));
        if (next_operation == RIGHT_BRACKET)
            ; /* next_operation has been reset */
        else  /* next_operation is now a bop2, plus-like */
            CHP(read_rest_bop2(line, counter, value, next_operation, parameters));
    }
    else /* next operation is a bop2, plus-like */
        CHP(read_rest_bop2(line, counter, value, next_operation, parameters));
    return INTERP_OK;
}
#endif

/****************************************************************************/

/*! read_real_expression

 The following version is stack-based and fully general. It is the
 classical stack-based version with left-to-right evaluation of
 operations of the same precedence. Separate stacks are used for
 operations and values, and the stacks are made with arrays
 rather than lists, but those are implementation details. Pushing
 and popping are implemented by increasing or decreasing the
 stack index.

 Additional levels of precedence may be defined easily by changing the
 precedence function. The size of MAX_STACK should always be at least
 as large as the number of precedence levels used. We are currently
 using four precedence levels (for right-bracket, plus-like operations,
 times-like operations, and power).

 N.B.: We are now using six levels (right-bracket, logical operations,
 relational operations, plus-like operations, times-like operations, and
 power).

 */

#define MAX_STACK 7

int Interp::read_real_expression(char* line,          //!< string: line of RS274/NGC code being processed
                                 int* counter,        //!< pointer to a counter for position on the line
                                 double* value,       //!< pointer to double to be computed
                                 double* parameters)  //!< array of system parameters
{
    jaka_value values[MAX_STACK];
    int operators[MAX_STACK];
    int stack_index;
    int ret;

    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = (*counter + 1);

    ret = read_real_value(line, counter, values, parameters);
    CHK_INTERP_READ(ret, line, counter);

    CHP(read_operation(line, counter, operators));
    stack_index = 1;
    for (; operators[0] != RIGHT_BRACKET;)
    {
        ret = read_real_value(line, counter, values + stack_index, parameters);
        CHK_INTERP_READ(ret, line, counter);

        CHP(read_operation(line, counter, operators + stack_index));
        if (precedence(operators[stack_index]) > precedence(operators[stack_index - 1]))
            stack_index++;
        else
        { /* precedence of latest operator is <= previous precedence */
            for (; precedence(operators[stack_index]) <= precedence(operators[stack_index - 1]);)
            {
                // rtapi_print("left value is %lf, right value is %lf .\n\n", values[stack_index - 1].scalarValue, values[stack_index].scalarValue);
                // CHP(execute_binary(&(values[stack_index - 1].scalarValue), operators[stack_index - 1], &(values[stack_index].scalarValue)));
                CHP(execute_binary(values[stack_index - 1], operators[stack_index - 1], values[stack_index]));
                operators[stack_index - 1] = operators[stack_index];
                if ((stack_index > 1) && (precedence(operators[stack_index - 1]) <= precedence(operators[stack_index - 2])))
                    stack_index--;
                else
                    break;
            }
        }
    }
    *value = values[0].scalarValue;
    return INTERP_OK;
}

/****************************************************************************/

/*! read_real_number

 Returned Value: int
 If any of the following errors occur, this returns the error shown.
 Otherwise, it returns INTERP_OK.
 1. The first character is not "+", "-", "." or a digit:
 NCE_BAD_NUMBER_FORMAT
 2. No digits are found after the first character and before the
 end of the line or the next character that cannot be part of a real:
 NCE_NO_DIGITS_FOUND_WHERE_REAL_NUMBER_SHOULD_BE
 3. sscanf fails: NCE_SSCANF_FAILED

 Side effects:
 The number read from the line is put into what double_ptr points at.
 The counter is reset to point to the first character after the real.

 Called by:  read_real_value

 This attempts to read a number out of the line, starting at the index
 given by the counter. It stops when the first character that cannot
 be part of the number is found.

 The first character may be a digit, "+", "-", or "."
 Every following character must be a digit or "." up to anything
 that is not a digit or "." (a second "." terminates reading).

 This function is not called if the first character is NULL, so it is
 not necessary to check that.

 The temporary insertion of a NULL character on the line is to avoid
 making a format string like "%3lf" which the LynxOS compiler cannot
 handle.

 */

int Interp::read_real_number(char* line,          //!< string: line of RS274/NGC code being processed
                             int* counter,        //!< pointer to a counter for position on the line
                             double* double_ptr)  //!< pointer to double to be read
{
    char* start;
    size_t after;

    start = line + *counter;
    after = strspn(start, "+-");
    after = strspn(start + after, "0123456789.") + after;

    std::string st(start, start + after);
    std::stringstream s(st);
    double val;
    if (!(s >> val))
    {  // 单条log的最大长度为512 Byte，如果error mesagge 过长会导致后面附加的脚本报错跳转相关的脚本文件名、行号信息在写入errlog时被丢弃。
        // ERS(INTERP_ERR_BAD_NUMBER_FORMAT, _("bad number format (conversion failed) parsing '%s'"), st.c_str());
        ERS(INTERP_ERR_BAD_NUMBER_FORMAT, _("bad number format (conversion failed) parsing "));
        printf("bad number format (conversion failed) parsing '%s'", st.c_str());
    }

    if (s.get() != std::char_traits<char>::eof())
    {
        // ERS(INTERP_ERR_BAD_NUMBER_FORMAT, _("bad number format (trailing characters) parsing '%s'"), st.c_str());
        ERS(INTERP_ERR_BAD_NUMBER_FORMAT, _("bad number format (trailing characters) parsing "));
        printf("bad number format (trailing characters) parsing '%s'", st.c_str());
    }

    *double_ptr = val;
    *counter = start + after - line;
    return INTERP_OK;
}

/****************************************************************************/

/*! read_real_value

 Returned Value: int
 If one of the following functions returns an error code,
 this returns that code.
 read_real_expression
 read_parameter
 read_unary
 read_real_number
 If no characters are found before the end of the line this
 returns NCE_NO_CHARACTERS_FOUND_IN_READING_REAL_VALUE.
 Otherwise, this returns INTERP_OK.

 Side effects:
 The value read from the line is put into what double_ptr points at.
 The counter is reset to point to the first character after the
 characters which make up the value.

 Called by:
 read_a
 read_b
 read_c
 read_f
 read_g
 read_i
 read_integer_value
 read_j
 read_k
 read_p
 read_parameter_setting
 read_q
 read_r
 read_real_expression
 read_s
 read_x
 read_y
 read_z

 This attempts to read a real value out of the line, starting at the
 index given by the counter. The value may be a number, a parameter
 value, a unary function, or an expression. It calls one of four
 other readers, depending upon the first character.

 */

int Interp::read_real_value(char* line,             //!< string: line of RS274/NGC code being processed
                            int* counter,           //!< pointer to a counter for position on the line
                            jaka_value* value_ptr,  //!< pointer to double to be read
                            double* parameters)     //!< array of system parameters
{
    int ret = 0;
    char c, c1;

    c = line[*counter];
    CHKS((c == 0), INTERP_ERR_SYNTAX_FAILED, NCE_NO_CHARACTERS_FOUND_IN_READING_REAL_VALUE);

    c1 = line[*counter + 1];

    if (c == '(')
    {
        ret = read_real_expression(line, counter, &(value_ptr->scalarValue), parameters);
        CHK_INTERP_READ(ret, line, counter);
    }
    else if (c == '+' && c1 && !isdigit(c1) && c1 != '.')
    {
        (*counter)++;
        ret = read_real_value(line, counter, value_ptr, parameters);
        CHK_INTERP_READ(ret, line, counter);
    }
    else if (c == '-' && c1 && !isdigit(c1) && c1 != '.')
    {
        (*counter)++;
        ret = read_real_value(line, counter, value_ptr, parameters);
        CHK_INTERP_READ(ret, line, counter);
        value_ptr->valueType = SCALAR_VALUE;
        value_ptr->scalarValue = -value_ptr->scalarValue;
    }
    else if (c == '\"')
    {
        ret = read_string_value(line, counter, value_ptr, parameters);
        CHK_INTERP_READ(ret, line, counter);
    }
    else if (c == '{' || c == '[')
    {
        ret = read_array_value(line, counter, value_ptr, parameters);
        CHK_INTERP_READ(ret, line, counter);
    }
    else if (((c >= 'a') && (c <= 'z')) || c == '_')
    {  // try built-in function first
        // rtapi_print("1: line is: %s, *counter = %d, line[*counter] = %c\n", line, *counter, line[*counter]);
        ret = read_unary(line, counter, value_ptr, parameters);
        if (ret == INTERP_NOT_HANDLE)
        {
            // rtapi_print("2: line is: %s, *counter = %d, line[*counter] = %c\n", line, *counter, line[*counter]);
            ret = read_parameter(line, counter, value_ptr, parameters, false);
            // rtapi_print("2: line is: %s, *counter = %d, valType = %d, size = %d\n", line, *counter, value_ptr->valueType, value_ptr->arrValue.size());
        }
        CHK_INTERP_READ(ret, line, counter);
    }
    else
    {
        value_ptr->valueType = SCALAR_VALUE;
        CHP(read_real_number(line, counter, &(value_ptr->scalarValue)));
    }

    CHKS(std::isnan(value_ptr->scalarValue), INTERP_ERR_SYNTAX_FAILED, _("Calculation resulted in 'not a number'"));
    CHKS(std::isinf(value_ptr->scalarValue), INTERP_ERR_SYNTAX_FAILED, _("Calculation resulted in 'infinity'"));

    return ret;
}

int Interp::read_array_value(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    CHKS((line[*counter] != '{' && line[*counter] != '['), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;
    if (line[*counter] == '}' || line[*counter] == ']')
    {
        value_ptr->valueType = ARRAY_VALUE;
        value_ptr->arrValue.clear();
        return INTERP_OK;
    }

    int ret;
    char arrValueText[LINELEN];
    strcpy(arrValueText, line + *counter);
    std::vector<double> arrVals;

    bool bArrClosed = false;
    const char* split = ",";
    char* arrEle = strtok(arrValueText, split);
    while (arrEle != NULL)
    {
        jaka_value eleValue;
        int subCounter = 0;
        while (subCounter < (int)strlen(arrEle) && arrEle[subCounter] == ' ')  // skip the space
            ++subCounter;

        ret = read_real_value(arrEle, &subCounter, &eleValue, parameters);
        CHK_INTERP_READ(ret, line, counter);

        CHKS(eleValue.valueType != SCALAR_VALUE, INTERP_ERR_SYNTAX_FAILED, _("Array is not allowed to be nested!"));
        arrVals.push_back(eleValue.scalarValue);

        if (strstr(arrEle, "}") != NULL || strstr(arrEle, "]") != NULL)
        {  // contain the '}'
            bArrClosed = true;
            break;
        }

        arrEle = strtok(NULL, split);
    }

    CHKS(!bArrClosed, INTERP_ERR_SYNTAX_FAILED, _("} is required to close the array definition!"));
    value_ptr->valueType = ARRAY_VALUE;
    value_ptr->arrValue.assign(arrVals.begin(), arrVals.end());
    // rtapi_print("array read: first is %f, last is %f\n", arrVals[0], arrVals[arrVals.size()-1]);
    return INTERP_OK;
}

int Interp::read_string_value(char* line, int* counter, jaka_value* value_ptr, double* parameters)
{
    *value_ptr = "";
    CHKS((line[*counter] != '\"'), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = *counter + 1;

    // int ret;
    bool bStrClosed = false;
    for (; *counter < (int)strlen(line); ++*counter)
    {
        if (line[*counter] == '\"')
        {
            bStrClosed = true;
            break;
        }
        else if (line[*counter] == '\\')
        {
            // 转义字符处理, \\, \', \", \n, \t, \r
            if (*counter + 1 < (int)strlen(line))
            {
                *counter += 1;
                switch (line[*counter])
                {
                case 'n':
                    value_ptr->strValue.push_back('\n');
                    break;
                case 'r':
                    value_ptr->strValue.push_back('\r');
                    break;
                case 't':
                    value_ptr->strValue.push_back('\t');
                    break;
                case '\"':
                    value_ptr->strValue.push_back('\"');
                    break;
                case '\'':
                    value_ptr->strValue.push_back('\'');
                    break;
                case '\\':
                    value_ptr->strValue.push_back('\\');
                    break;
                default:
                    ERS(INTERP_ERR_UNSUPPORTED_ESC_CHARATER, _("unsupported escape characters!"));
                }
            }
            else
            {
                ERS(INTERP_ERR_SYNTAX_FAILED, _("\" is required to close the string definition!"));
            }
        }
        else
        {
            value_ptr->strValue.push_back(line[*counter]);
        }
    }
    CHKS(!bStrClosed, INTERP_ERR_SYNTAX_FAILED, _("\" is required to close the string definition!"));
    // rtapi_print("string value: %s\n", value_ptr->strValue.c_str());
    return INTERP_OK;
}

/****************************************************************************/

/*! read_rest_bop1
 }

 Returned Value: int
 If any of the following functions returns an error code,
 this returns that code.
 execute_binary1
 read_real_value
 read_operation
 Otherwise, it returns INTERP_OK.

 Side effects:
 The value argument is set to the value of the expression.
 The counter is reset to point to the first character after the real
 expression.

 Called by:
 read_real_expression
 read_rest_bop2

 The value argument has a value in it when this is called. This repeatedly
 gets the next_value and the next_operation, performs the last_operation
 on the value and the next_value and resets the last_operation to the
 next_operation. Observe that both the value and the last_operation
 are passed back to the caller.

 This is commented out since it is not used in the uncommented version
 of read_real_expression. It has been tested.

 */

#ifdef UNDEFINED
int Interp::read_rest_bop1(char* line,           //!< string: line of RS274/NGC code being processed
                           int* counter,         //!< pointer to a counter for position on the line
                           double* value,        //!< pointer to double to be calculated
                           int* last_operation,  //!< last operation read, reset to next operation
                           double* parameters)   //!< array of system parameters
{
    static char name[] = "read_rest_bop1";
    double next_value;
    int next_operation;
    int status;
    int ret;

    for (;;)
    {
        ret = read_real_value(line, counter, &next_value, parameters);
        CHK_INTERP_READ(ret, line, counter);

        CHP(read_operation(line, counter, &next_operation));
        CHP(execute_binary1(value, *last_operation, &next_value));
        *last_operation = next_operation;
        if (next_operation >= AND2) /* next op is a bop2 or right bracket */
            break;
    }
    return INTERP_OK;
}
#endif

/****************************************************************************/

/*! read_rest_bop2

 Returned Value: int
 If any of the following functions returns an error code,
 this returns that code.
 execute_binary2
 read_real_value
 read_operation
 read_rest_bop1
 Otherwise, it returns INTERP_OK.

 Side effects:
 The value argument is set to the value of the expression.
 The counter is reset to point to the first character after the real
 expression.

 Called by:  read_real_expression

 The value argument has a value in it when this is called. This repeatedly
 gets the next_value and the next_operation, performs the last_operation
 on the value and the next_value and resets the last_operation to the
 next_operation. If the next_operation is ever a bop1 read_rest_bop1 is
 called to set the next_value.

 This is commented out since it is not used in the uncommented version
 of read_real_expression. It has been tested.

 */

#ifdef UNDEFINED
int Interp::read_rest_bop2(char* line,          //!< string: line of RS274/NGC code being processed
                           int* counter,        //!< pointer to a counter for position on the line
                           double* value,       //!< pointer to double to be calculated
                           int last_operation,  //!< last operation read
                           double* parameters)  //!< array of system parameters
{
    static char name[] = "read_rest_bop2";
    double next_value;
    int next_operation;
    int status;
    int ret;

    for (;; last_operation = next_operation)
    {
        ret = read_real_value(line, counter, &next_value, parameters);
        CHK_INTERP_READ(ret, line, counter);

        CHP(read_operation(line, counter, &next_operation));
        if (next_operation < AND2)
        { /* next operation is a bop1 */
            ret = read_rest_bop1(line, counter, &next_value, &next_operation, parameters);
            CHK_INTERP_READ(ret, line, counter);
        }
        CHP(execute_binary2(value, last_operation, &next_value));
        if (next_operation == RIGHT_BRACKET)
            break;
    }
    return INTERP_OK;
}
#endif

/****************************************************************************/

/*! read_s

 Returned Value: int
 If read_real_value returns an error code, this returns that code.
 If any of the following errors occur, this returns the error code shown.
 Otherwise, it returns INTERP_OK.
 1. The first character read is not s:
 NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED
 2. A spindle speed has already been inserted in the block:
 NCE_MULTIPLE_S_WORDS_ON_ONE_LINE
 3. The spindle speed is negative: NCE_NEGATIVE_SPINDLE_SPEED_USED

 Side effects:
 counter is reset to the character following the spindle speed.
 A spindle speed setting is inserted in the block.

 Called by: read_one_item

 When this function is called, counter is pointing at an item on the
 line that starts with the character 's', indicating a spindle speed
 setting. The function reads characters which tell how to set the spindle
 speed.

 The value may be a real number or something that evaluates to a
 real number, so read_real_value is used to read it. Parameters
 may be involved.

 */

int Interp::read_s(char* line,           //!< string: line of RS274NGC code being processed
                   int* counter,         //!< pointer to a counter for position on the line
                   block_pointer block,  //!< pointer to a block being filled from the line
                   double* parameters)   //!< array of system parameters
{
    jaka_value value;

    CHKS((line[*counter] != 's'), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = (*counter + 1);
    CHKS((block->s_flag), INTERP_ERR_SYNTAX_FAILED, NCE_MULTIPLE_S_WORDS_ON_ONE_LINE);
    CHP(read_real_value(line, counter, &value, parameters));
    CHKS((value.scalarValue < 0.0), INTERP_ERR_SYNTAX_FAILED, NCE_NEGATIVE_SPINDLE_SPEED_USED);
    block->s_number = value.scalarValue;
    block->s_flag = true;
    return INTERP_OK;
}

/****************************************************************************/

/*! read_t

 Returned Value: int
 If read_integer_value returns an error code, this returns that code.
 If any of the following errors occur, this returns the error code shown.
 Otherwise, it returns INTERP_OK.
 1. The first character read is not t:
 NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED
 2. A t_number has already been inserted in the block:
 NCE_MULTIPLE_T_WORDS_ON_ONE_LINE
 3. The t_number is negative: NCE_NEGATIVE_TOOL_ID_USED

 Side effects:
 counter is reset to the character following the t_number.
 A t_number is inserted in the block.

 Called by: read_one_item

 When this function is called, counter is pointing at an item on the
 line that starts with the character 't', indicating a tool.
 The function reads characters which give the (integer) value of the
 tool code.

 The value must be an integer or something that evaluates to a
 real number, so read_integer_value is used to read it. Parameters
 may be involved.

 */

int Interp::read_t(char* line,           //!< string: line of RS274/NGC code being processed
                   int* counter,         //!< pointer to a counter for position on the line
                   block_pointer block,  //!< pointer to a block being filled from the line
                   double* parameters)   //!< array of system parameters
{
    int value;

    CHKS((line[*counter] != 't'), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = (*counter + 1);
    CHKS((block->t_flag), INTERP_ERR_SYNTAX_FAILED, NCE_MULTIPLE_T_WORDS_ON_ONE_LINE);
    CHP(read_integer_value(line, counter, &value, parameters));
    CHKS((value < 0), INTERP_ERR_SYNTAX_FAILED, NCE_NEGATIVE_TOOL_ID_USED);
    block->t_number = value;
    block->t_flag = true;
    return INTERP_OK;
}

/****************************************************************************/

/*! read_text

 Returned Value: int
 If close_and_downcase returns an error code, this returns that code.
 If any of the following errors occur, this returns the error code shown.
 Otherwise, this returns:
 a. INTERP_ENDFILE if the percent_flag is true and the only
 non-white character on the line is %,
 b. INTERP_EXECUTE_FINISH if the first character of the
 close_and_downcased line is a slash, and
 c. INTERP_OK otherwise.
 1. The end of the file is found and the percent_flag is true:
 NCE_FILE_ENDED_WITH_NO_PERCENT_SIGN
 2. The end of the file is found and the percent_flag is false:
 NCE_FILE_ENDED_WITH_NO_PERCENT_SIGN_OR_PROGRAM_END
 3. The command argument is not null and is too long or the command
 argument is null and the line read from the file is too long:
 NCE_COMMAND_TOO_LONG

 Side effects: See below

 Called by:  Interp::read

 This reads a line of RS274 code from a command string or a file into
 the line array. If the command string is not null, the file is ignored.

 If the end of file is reached, an error is returned as described
 above. The end of the file should not be reached because (a) if the
 file started with a percent line, it must end with a percent line, and
 no more reading of the file should occur after that, and (b) if the
 file did not start with a percent line, it must have a program ending
 command (M2 or M30) in it, and no more reading of the file should
 occur after that.

 All blank space at the end of a line read from a file is removed and
 replaced here with NULL characters.

 This then calls close_and_downcase to downcase and remove tabs and
 spaces from everything on the line that is not part of a comment. Any
 comment is left as is.

 The length is set to zero if any of the following occur:
 1. The line now starts with a slash, but the second character is NULL.
 2. The first character is NULL.
 Otherwise, length is set to the length of the line.

 An input line is blank if the first character is NULL or it consists
 entirely of tabs and spaces and, possibly, a newline before the first
 NULL.

 Block delete is discussed in [NCMS, page 3] but the discussion makes
 no sense. Block delete is handled by having this function return
 INTERP_EXECUTE_FINISH if the first character of the
 close_and_downcased line is a slash. When the caller sees this,
 the caller is expected not to call Interp::execute if the switch
 is on, but rather call Interp::read again to overwrite and ignore
 what is read here.

 The value of the length argument is set to the number of characters on
 the reduced line.

 */

int Interp::read_text(const char* command,  //!< a string which may have input text, or null
                      FILE* inport,         //!< a file pointer for an input file, or null
                      char* raw_line,       //!< array to write raw input line into
                      char* line,           //!< array for input line to be processed in
                      int* length)          //!< a pointer to an integer to be set
{
    int index;
    if (command == NULL)
    {
        if (fgets(raw_line, LINELEN, inport) == NULL)
        {
            if (_setup.skipping_to_sub)
            {
                printf("EOF in file:%s seeking o-word: o<%s> from line: %d", _setup.filename, _setup.skipping_to_sub, _setup.skipping_start);
                ERS(INTERP_ERR_TEMP, _("EOF in file"));
            }

            // check the end map when file is ended
            if (m_owordIdMap.size() != 0 /* && _setup.sub_filename.empty() */)
            {
                ERS(INTERP_ERR_UNMATCHED_STATMENT_END, "control word %d doesn't match an end", m_owordIdMap.back().first);
            }
            // rtapi_print("raw_line---------------------------\n");
            memcpy(raw_line, "m2", 3 * sizeof(char));
            _setup.sequence_number--;
        }
        _setup.sequence_number++; /* moved from version1, was outside if */
        if (strlen(raw_line) == (LINELEN - 1))
        {                                                              // line is too long. need to finish reading the line to recover
            for (; fgetc(inport) != '\n' && fgetc(inport) != EOF;) {}  // could also look for EOF
            ERS(INTERP_ERR_COMMAND_TOO_LONG, NCE_COMMAND_TOO_LONG);
        }
        for (index = (strlen(raw_line) - 1);  // index set on last char
             (index >= 0) && (isspace(raw_line[index]));
             index--)
        {  // remove space at end of raw_line, especially CR & LF
            raw_line[index] = 0;
        }
        strcpy(line, raw_line);
        CHP(close_and_downcase(line));
        if ((line[0] == '%') && (line[1] == 0) && (_setup.percent_flag))
        {
            FINISH();
            rtapi_print("222222222222222222222222222222\n");
            return INTERP_ENDFILE;
        }
    }
    else
    {
        CHKS((strlen(command) >= LINELEN), INTERP_ERR_SYNTAX_FAILED, NCE_COMMAND_TOO_LONG);
        strcpy(raw_line, command);
        strcpy(line, command);
        CHP(close_and_downcase(line));
    }

    _setup.parameter_occurrence = 0; /* initialize parameter buffer */

    if ((line[0] == 0) /* || ((line[0] == '/') && (GET_BLOCK_DELETE())) */)
        *length = 0;
    else
        *length = strlen(line);
    return INTERP_OK;
}

/****************************************************************************/

/*! read_unary

 Returned Value: int
 If any of the following functions returns an error code,
 this returns that code.
 execute_unary
 read_atan
 read_operation_unary
 read_real_expression
 If any of the following errors occur, this returns the error code shown.
 Otherwise, it returns INTERP_OK.
 1. the name of the unary operation is not followed by a left bracket:
 NCE_LEFT_BRACKET_MISSING_AFTER_UNARY_OPERATION_NAME

 Side effects:
 The value read from the line is put into what double_ptr points at.
 The counter is reset to point to the first character after the
 characters which make up the value.

 Called by:  read_real_value

 This attempts to read the value of a unary operation out of the line,
 starting at the index given by the counter. The atan operation is
 handled specially because it is followed by two arguments.

 */

int Interp::read_unary(char* line,             //!< string: line of RS274/NGC code being processed
                       int* counter,           //!< pointer to a counter for position on the line
                       jaka_value* value_ptr,  //!< pointer to double to be read
                       double* parameters)     //!< array of system parameters
{
    int operation;
    int ret;
    // rtapi_print("read_unary: line is %s, *counter=%d\n", line, *counter);
    ret = read_operation_unary(line, counter, &operation);
    // rtapi_print("after read_unary: line is %s, *counter=%d\n", line, *counter);
    if (ret == INTERP_NOT_HANDLE)
    {
        return ret;
    }

    CHP(ret);
    CHKS((line[*counter] != '('), INTERP_ERR_SYNTAX_FAILED, NCE_LEFT_BRACKET_MISSING_AFTER_UNARY_OPERATION_NAME);

    if (operation == EXISTS)
    {
        CHP(read_bracketed_parameter(line, counter, value_ptr, parameters, true));
        return INTERP_OK;
    }
    else if (operation == ATAN2)
    {
        ret = read_atan2(line, counter, &(value_ptr->scalarValue), parameters);
        return ret;
    }
    else if (operation == POSEADD)
    {
        CHP(read_poseadd(line, counter, value_ptr, parameters));
        return INTERP_OK;
    }
    else if (operation == KININVERSE)
    {
        CHP(read_kinInverse(line, counter, value_ptr, parameters));
        return INTERP_OK;
    }
    else if (operation == KINFORWARD)
    {
        CHP(read_kinForward(line, counter, value_ptr, parameters));
        return INTERP_OK;
    }
    else if (operation == POSEREACHABLE)
    {
        CHP(read_isPoseReachable(line, counter, value_ptr, parameters));
        return INTERP_OK;
    }
    else if (operation == POSESUB)
    {
        CHP(read_posesub(line, counter, value_ptr, parameters));
        return INTERP_OK;
    }
    else if (operation == POSEINV)
    {
        CHP(read_poseinv(line, counter, value_ptr, parameters));
        return INTERP_OK;
    }
    else if (operation == POSEDIST)
    {
        CHP(read_posedist(line, counter, value_ptr, parameters));
        return INTERP_OK;
    }
    else if (operation == POSETRANS)
    {
        CHP(read_posetrans(line, counter, value_ptr, parameters));
        return INTERP_OK;
    }
    else if (operation == POSEINTPL)
    {
        CHP(read_poseintpl(line, counter, value_ptr, parameters));
        return INTERP_OK;
    }
    else if (operation == XYPLANETRANS)
    {
        CHP(read_xyplanetrans(line, counter, value_ptr, parameters));
        return INTERP_OK;
    }
    else if (operation == YZPLANETRANS)
    {
        CHP(read_yzplanetrans(line, counter, value_ptr, parameters));
        return INTERP_OK;
    }
    else if (operation == ZXPLANETRANS)
    {
        CHP(read_zxplanetrans(line, counter, value_ptr, parameters));
        return INTERP_OK;
    }
    else if (operation == GETDOUT)
    {
        ret = read_getdout(line, counter, &(value_ptr->scalarValue), parameters);
        return ret;
    }
    else if (operation == GETAOUT)
    {
        ret = read_getaout(line, counter, &(value_ptr->scalarValue), parameters);
        return ret;
    }
    else if (operation == GETDIN)
    {
        ret = read_getdin(line, counter, &(value_ptr->scalarValue), parameters);
        return ret;
    }
    else if (operation == GETAIN)
    {
        ret = read_getain(line, counter, &(value_ptr->scalarValue), parameters);
        return ret;
    }
    else if (operation == GETATLJOINTPOSE)
    {
        ret = read_getatljointpose(line, counter, value_ptr, parameters);
        return ret;
    }
    else if (operation == GETATLTCPPOSE)
    {
        ret = read_getatltcppose(line, counter, value_ptr, parameters);
        return ret;
    }
    else if (operation == GETATLFLANGEPOSE)
    {
        ret = read_getatlflangepose(line, counter, value_ptr, parameters);
        return ret;
    }
    else if (operation == GETTCPOFFSETSOF)
    {
        ret = read_gettooloffsetsof(line, counter, value_ptr, parameters);
        return ret;
    }
    else if (operation == GETTCPOFFSETS)
    {
        ret = read_gettooloffsets(line, counter, value_ptr, parameters);
        return ret;
    }
    else if (operation == GETUSEROFFSETSOF)
    {
        ret = read_getuseroffsetsof(line, counter, value_ptr, parameters);
        return ret;
    }
    else if (operation == GETCLSNSENSITIVITY)
    {
        ret = read_getclsnsensitivity(line, counter, value_ptr, parameters);
        return ret;
    }
    else if (operation == GETPAYLOAD)
    {
        ret = read_getpayload(line, counter, value_ptr, parameters);
        return ret;
    }
    else if (operation == GETSENSORTORQUE)
    {
        ret = read_getsensortorque(line, counter, value_ptr, parameters);
        return ret;
    }
    else if (operation == GETUSEROFFSETS)
    {
        ret = read_getuseroffsets(line, counter, value_ptr, parameters);
        return ret;
    }
    else if (operation == GETTIMEOUT)
    {
        ret = read_gettimeout(line, counter, &(value_ptr->scalarValue), parameters);
        return ret;
        // } else if (operation == GETARRAYELEMENT) {
        // 	ret = read_getarrayelement(line, counter, value_ptr, parameters);
        // 	return ret;
    }
    else if (operation == GETLENGTH)
    {
        ret = read_getlength(line, counter, value_ptr, parameters);
        return ret;
    }
    else if (operation == GETARRAYFROMSTRING)
    {
        ret = read_getarrayfromstring(line, counter, value_ptr, parameters);
        return ret;
    }
    else if (operation == GETSTRINGFROMARRAY)
    {
        ret = read_getstringfromarray(line, counter, value_ptr, parameters);
        return ret;
    }
    else if (operation == SOCKETOPEN)
    {
        int expres = 0;
        ret = read_socketopen(line, counter, &expres, parameters);
        value_ptr->valueType = SCALAR_VALUE;
        value_ptr->scalarValue = expres;
        return ret;
    }
    else if (operation == SOCKETGETVAR)
    {
        value_ptr->valueType = SCALAR_VALUE;
        value_ptr->scalarValue = 0;
        ret = read_socketgetvar(line, counter, value_ptr, parameters);
        return ret;
    }
    else if (operation == SOCKETRECV)
    {
        value_ptr->valueType = STRING_VALUE;
        value_ptr->strValue = std::string("");
        ret = read_socketrecv(line, counter, value_ptr, parameters);
        return ret;
    }
    else if (operation == SOCKETREADREAL)
    {
        value_ptr->valueType = SCALAR_VALUE;
        value_ptr->scalarValue = 0;
        ret = read_socketreadreal(line, counter, value_ptr, parameters);
        return ret;
    }
    else if (operation == SOCKETREADSTRING)
    {
        value_ptr->valueType = STRING_VALUE;
        value_ptr->strValue = std::string("");
        ret = read_socketreadstring(line, counter, value_ptr, parameters);
        return ret;
    }
    else if (operation == SOCKETSEND)
    {
        value_ptr->valueType = SCALAR_VALUE;
        value_ptr->scalarValue = 0;
        ret = read_socketsend(line, counter, value_ptr, parameters);
        return ret;
    }
    else if (operation == SOCKETSENDBYTE)
    {
        value_ptr->valueType = SCALAR_VALUE;
        value_ptr->scalarValue = 0;
        ret = read_socketsendbyte(line, counter, value_ptr, parameters);
        return ret;
    }
    else if (operation == SPRINTF)
    {
        ret = read_sprintf(line, counter, value_ptr, parameters);
        return ret;
    }
    else if (operation == SSCANF)
    {
        ret = read_sscanf(line, counter, value_ptr, parameters);
        return ret;
    }
    else if (operation == STRINGCONCAT)
    {
        ret = read_stringconcat(line, counter, value_ptr, parameters);
        return ret;
    }
    else if (operation == STRINGCMP)
    {
        ret = read_strcmp(line, counter, value_ptr, parameters);
        return ret;
    }
    else if (operation == GETSYSCLOCK)
    {
        ret = read_getsysclock(line, counter, value_ptr, parameters);
        return ret;
    }
    else if (operation == GETTIOSIGNALVAL)
    {
        ret = read_tiogetsignalvalue(line, counter, value_ptr, parameters);
        return ret;
    }

    value_ptr->valueType = SCALAR_VALUE;
    ret = read_real_expression(line, counter, &(value_ptr->scalarValue), parameters);
    CHK_INTERP_READ(ret, line, counter);
    ret = execute_unary(&(value_ptr->scalarValue), operation);

    CHK_INTERP_READ(ret, line, counter);

    return INTERP_OK;
}

int Interp::read_u(char* line,           //!< string: line of RS274/NGC code being processed
                   int* counter,         //!< pointer to a counter for position on the line
                   block_pointer block,  //!< pointer to a block being filled from the line
                   double* parameters)   //!< array of system parameters
{
    jaka_value value;

    CHKS((line[*counter] != 'u'), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = (*counter + 1);
    CHKS((block->u_flag), INTERP_ERR_SYNTAX_FAILED, _("Multiple U words on one line"));
    CHP(read_real_value(line, counter, &value, parameters));
    block->u_flag = true;
    block->u_number = value.scalarValue;
    return INTERP_OK;
}

int Interp::read_v(char* line,           //!< string: line of RS274/NGC code being processed
                   int* counter,         //!< pointer to a counter for position on the line
                   block_pointer block,  //!< pointer to a block being filled from the line
                   double* parameters)   //!< array of system parameters
{
    jaka_value value;

    CHKS((line[*counter] != 'v'), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = (*counter + 1);
    CHKS((block->v_flag), INTERP_ERR_SYNTAX_FAILED, _("Multiple V words on one line"));
    CHP(read_real_value(line, counter, &value, parameters));
    block->v_flag = true;
    block->v_number = value.scalarValue;
    return INTERP_OK;
}

int Interp::read_w(char* line,           //!< string: line of RS274/NGC code being processed
                   int* counter,         //!< pointer to a counter for position on the line
                   block_pointer block,  //!< pointer to a block being filled from the line
                   double* parameters)   //!< array of system parameters
{
    jaka_value value;

    CHKS((line[*counter] != 'w'), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = (*counter + 1);
    CHKS((block->w_flag), INTERP_ERR_SYNTAX_FAILED, _("Multiple W words on one line"));
    CHP(read_real_value(line, counter, &value, parameters));
    block->w_flag = true;
    block->w_number = value.scalarValue;
    return INTERP_OK;
}

/****************************************************************************/

/*! read_x

 Returned Value: int
 If read_real_value returns an error code, this returns that code.
 If any of the following errors occur, this returns the error code shown.
 Otherwise, it returns INTERP_OK.
 1. The first character read is not x:
 NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED
 2. A x_coordinate has already been inserted in the block:
 NCE_MULTIPLE_X_WORDS_ON_ONE_LINE

 Side effects:
 counter is reset.
 The x_flag in the block is turned on.
 An x_number is inserted in the block.

 Called by: read_one_item

 When this function is called, counter is pointing at an item on the
 line that starts with the character 'x', indicating a x_coordinate
 setting. The function reads characters which tell how to set the
 coordinate, up to the start of the next item or the end of the line.
 This information is inserted in the block. The counter is then set to
 point to the character following.

 The value may be a real number or something that evaluates to a
 real number, so read_real_value is used to read it. Parameters
 may be involved.

 */

int Interp::read_x(char* line,           //!< string: line of RS274 code being processed
                   int* counter,         //!< pointer to a counter for position on the line
                   block_pointer block,  //!< pointer to a block being filled from the line
                   double* parameters)   //!< array of system parameters
{
    jaka_value value;

    CHKS((line[*counter] != 'x'), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = (*counter + 1);
    CHKS((block->x_flag), INTERP_ERR_SYNTAX_FAILED, NCE_MULTIPLE_X_WORDS_ON_ONE_LINE);
    CHP(read_real_value(line, counter, &value, parameters));
    block->x_flag = true;
    if (_setup.lathe_diameter_mode)
    {
        block->x_number = value.scalarValue / 2;
    }
    else
    {
        block->x_number = value.scalarValue;
    }
    return INTERP_OK;
}

int Interp::read_atsign(char* line, int* counter, block_pointer block, double* parameters)
{
    CHKS((line[*counter] != '@'), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    (*counter)++;
    jaka_value tempValue;
    CHP(read_real_value(line, counter, &tempValue, parameters));
    block->radius = tempValue.scalarValue;
    block->radius_flag = true;
    return INTERP_OK;
}

int Interp::read_carat(char* line, int* counter, block_pointer block, double* parameters)
{
    CHKS((line[*counter] != '^'), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    (*counter)++;
    jaka_value tempValue;
    CHP(read_real_value(line, counter, &tempValue, parameters));
    block->theta = tempValue.scalarValue;
    block->theta_flag = true;
    return INTERP_OK;
}

/****************************************************************************/

/*! read_y

 Returned Value: int
 If read_real_value returns an error code, this returns that code.
 If any of the following errors occur, this returns the error code shown.
 Otherwise, it returns INTERP_OK.
 1. The first character read is not y:
 NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED
 2. A y_coordinate has already been inserted in the block:
 NCE_MULTIPLE_Y_WORDS_ON_ONE_LINE

 Side effects:
 counter is reset.
 The y_flag in the block is turned on.
 A y_number is inserted in the block.

 Called by: read_one_item

 When this function is called, counter is pointing at an item on the
 line that starts with the character 'y', indicating a y_coordinate
 setting. The function reads characters which tell how to set the
 coordinate, up to the start of the next item or the end of the line.
 This information is inserted in the block. The counter is then set to
 point to the character following.

 The value may be a real number or something that evaluates to a
 real number, so read_real_value is used to read it. Parameters
 may be involved.

 */

int Interp::read_y(char* line,           //!< string: line of RS274 code being processed
                   int* counter,         //!< pointer to a counter for position on the line
                   block_pointer block,  //!< pointer to a block being filled from the line
                   double* parameters)   //!< array of system parameters
{
    jaka_value value;

    CHKS((line[*counter] != 'y'), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = (*counter + 1);
    CHKS((block->y_flag), INTERP_ERR_SYNTAX_FAILED, NCE_MULTIPLE_Y_WORDS_ON_ONE_LINE);
    CHP(read_real_value(line, counter, &value, parameters));
    block->y_flag = true;
    block->y_number = value.scalarValue;
    return INTERP_OK;
}

/****************************************************************************/

/*! read_z

 Returned Value: int
 If read_real_value returns an error code, this returns that code.
 If any of the following errors occur, this returns the error code shown.
 Otherwise, it returns INTERP_OK.
 1. The first character read is not z:
 NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED
 2. A z_coordinate has already been inserted in the block:
 NCE_MULTIPLE_Z_WORDS_ON_ONE_LINE

 Side effects:
 counter is reset.
 The z_flag in the block is turned on.
 A z_number is inserted in the block.

 Called by: read_one_item

 When this function is called, counter is pointing at an item on the
 line that starts with the character 'z', indicating a z_coordinate
 setting. The function reads characters which tell how to set the
 coordinate, up to the start of the next item or the end of the line.
 This information is inserted in the block. The counter is then set to
 point to the character following.

 The value may be a real number or something that evaluates to a
 real number, so read_real_value is used to read it. Parameters
 may be involved.

 */

int Interp::read_z(char* line,           //!< string: line of RS274 code being processed
                   int* counter,         //!< pointer to a counter for position on the line
                   block_pointer block,  //!< pointer to a block being filled from the line
                   double* parameters)   //!< array of system parameters
{
    jaka_value value;

    CHKS((line[*counter] != 'z'), INTERP_ERR_SYNTAX_FAILED, NCE_BUG_FUNCTION_SHOULD_NOT_HAVE_BEEN_CALLED);
    *counter = (*counter + 1);
    CHKS((block->z_flag), INTERP_ERR_SYNTAX_FAILED, NCE_MULTIPLE_Z_WORDS_ON_ONE_LINE);
    CHP(read_real_value(line, counter, &value, parameters));
    block->z_flag = true;
    block->z_number = value.scalarValue;
    return INTERP_OK;
}

bool Interp::isreadonly(int index)
{
    int i;
    for (i = 0; i < _n_readonly_parameters; i++)
    {
        if (_readonly_parameters[i] == index)
            return 1;
    }
    return 0;
}

/**
 * "def"
 * "call"
 * "do"
 * "while"
 * "repeat"
 * "if"
 * "elif"
 * "else"
 * "break"
 * "continue"
 * "return"
 * "end"
 * 
 * b: 	break;
 * c:	continue, 	call
 * d:	do,	def
 * e:	else,	elif,	end
 * i:	if
 * r:	return,	repeat
 * w: 	while
*/
int Interp::initOffsetMap(std::string filename)
{
    std::string cmd = "grep  -n -b -E "
                      "\"(^\\bdef\\b)|(^\\bcall\\b)|(^\\bdo\\b)|(^\\bwhile\\b)|(^\\brepeat\\b)|(^\\bif\\b)|(^\\belif\\b)|(^\\belse\\b)|(^\\bbreak\\b)|(^"
                      "\\bcontinue\\b)|(^\\breturn\\b)|(^\\bend\\b)\" ";
    std::string filePath = std::string(filename);
    cmd = cmd + "\"" + filePath + "\"" + " >  /tmp/blocks.jks";  // 避免filePath中有空格
    system(cmd.c_str());
    _setup.offset_map.clear();
    try
    {
        // const char *subName;
        std::ifstream inFile("/tmp/blocks.jks");
        std::string line;
        std::vector<std::pair<int, std::string>> _owordIdMap;
        while (getline(inFile, line))
        {
            std::vector<std::string> strList;
            stringSplit(line, strList, ":");
            int lineNum = std::atoi(strList.at(0).c_str());
            long lineOffset = std::atol(strList.at(1).c_str());

            const char* subName;
            char script[LINELEN + 1];
            char blockName[LINELEN + 1];
            strncpy(script, strList.at(2).c_str(), LINELEN);
            if (script[strList.at(2).length() - 1] == '\n' || script[strList.at(2).length() - 1] == '\r')
                script[strList.at(2).length() - 1] = '\0';

            int index = 0;
            bool is_token = true;
            while (script[index] == ' ' || script[index] == '\t') index++;

            ////FIXME  call_level ?
            if (_setup.call_level)
            {
                subName = _setup.sub_context[_setup.call_level].subName;
            }
            else if (_setup.defining_sub)
            {
                subName = _setup.sub_name;
            }
            else
            {
                subName = "";
            }
            switch (script[index])
            {
            case 'b': {
                if (isToken(script, &index, "break"))
                {
                    std::vector<std::pair<int, std::string>>::reverse_iterator iter;
                    for (iter = _owordIdMap.rbegin(); iter < _owordIdMap.rend(); iter++)
                    {
                        if (iter->first == O_do || iter->first == O_while)
                        {
                            break;
                        }
                    }
                    if (iter == _owordIdMap.rend())
                    {
                        rtapi_print("#>>>cannot find a matched loop start for break\n");
                        throw std::exception();
                    }
                    sprintf(blockName, "%s#%s", subName, iter->second.c_str());
                    offset new_offset = {O_break, strstore(_setup.filename), lineOffset, lineNum - 1, -1};
                    _setup.offset_map.insert(std::make_pair(strstore(blockName), new_offset));
                }
                else
                {
                    is_token = false;
                }
                break;
            }
            case 'c': {
                if (isToken(script, &index, "continue"))
                {
                    std::vector<std::pair<int, std::string>>::reverse_iterator iter;
                    for (iter = _owordIdMap.rbegin(); iter < _owordIdMap.rend(); iter++)
                    {
                        if (iter->first == O_do || iter->first == O_while)
                        {
                            break;
                        }
                    }
                    if (iter == _owordIdMap.rend())
                    {
                        rtapi_print(">>>cannot find a matched loop start for continue\n");
                        throw std::exception();
                    }
                    sprintf(blockName, "%s#%s", subName, iter->second.c_str());
                    offset new_offset = {O_continue, strstore(_setup.filename), lineOffset, lineNum - 1, -1};
                    _setup.offset_map.insert(std::make_pair(strstore(blockName), new_offset));
                }
                else if (isToken(script, &index, "call"))
                {
                    char subName[256];
                    CHP(read_name(script, &index, subName));
                    offset new_offset = {O_call, strstore(_setup.filename), lineOffset, lineNum - 1, -1};
                    _setup.offset_map.insert(std::make_pair(strstore(subName), new_offset));
                }
                else
                {
                    is_token = false;
                }
                break;
            }
            case 'd': {
                if (isToken(script, &index, "do"))
                {
                    std::pair<int, std::string> doItem = std::make_pair(O_do, std::to_string(_setup.sequence_number - 1));
                    _owordIdMap.push_back(doItem);

                    sprintf(blockName, "%s#%s", subName, doItem.second.c_str());
                    offset new_offset = {O_do, strstore(_setup.filename), lineOffset, lineNum - 1, -1};
                    _setup.offset_map.insert(std::make_pair(strstore(blockName), new_offset));
                }
                else if (isToken(script, &index, "def"))
                {
                    char subName[256];
                    CHP(read_name(script, &index, subName));
                    std::pair<int, std::string> subItem = std::make_pair(O_sub, std::string(subName));
                    _owordIdMap.push_back(subItem);

                    offset new_offset = {O_sub, strstore(_setup.filename), lineOffset, lineNum - 1, -1};
                    _setup.offset_map.insert(std::make_pair(strstore(subName), new_offset));
                }
                else
                {
                    is_token = false;
                }
                break;
            }
            case 'e': {
                if (isToken(script, &index, "end"))
                {
                    if (_owordIdMap.size())
                    {
                        std::pair<int, std::string> lastItem = _owordIdMap.back();
                        if (lastItem.first == O_while)
                        {
                            offset new_offset = {O_endwhile, strstore(_setup.filename), lineOffset, lineNum - 1, -1};
                            sprintf(blockName, "%s#%s", subName, lastItem.second.c_str());
                            _setup.offset_map.insert(std::make_pair(strstore(blockName), new_offset));
                        }
                        else if (lastItem.first == O_repeat)
                        {
                            offset new_offset = {O_endrepeat, strstore(_setup.filename), lineOffset, lineNum - 1, -1};
                            sprintf(blockName, "%s#%s", subName, lastItem.second.c_str());
                            _setup.offset_map.insert(std::make_pair(strstore(blockName), new_offset));
                        }
                        else if (lastItem.first == O_sub)
                        {
                            offset new_offset = {O_endsub, strstore(_setup.filename), lineOffset, lineNum - 1, -1};
                            sprintf(blockName, "%s", lastItem.second.c_str());
                            _setup.offset_map.insert(std::make_pair(strstore(blockName), new_offset));
                        }
                        else if (lastItem.first == O_if)
                        {
                            offset new_offset = {O_endif, strstore(_setup.filename), lineOffset, lineNum - 1, -1};
                            sprintf(blockName, "%s#%s", subName, lastItem.second.c_str());
                            _setup.offset_map.insert(std::make_pair(strstore(blockName), new_offset));
                            // rtapi_print(">>>>>>>>>>>>>>>>>>>>>>>>>>blockName=%s , lineNum=%d  lineOffset=%ld \n",blockName,new_offset.sequence_number,new_offset.offset);
                        }
                        else
                        {
                            rtapi_print("unmathced token 'end' \n");
                            throw std::exception();
                        }
                        _owordIdMap.pop_back();
                    }
                    else
                    {
                        rtapi_print("initOffsetMap : unmatched token end\n");
                        throw std::exception();
                    }
                }
                else if (isToken(script, &index, "else"))
                {
                    if (_owordIdMap.size() <= 0)
                    {
                        rtapi_print("initOffsetMap : unmatched token else \n");
                        throw std::exception();
                    }
                    std::pair<int, std::string> lastItem = _owordIdMap.back();
                    if (lastItem.first != O_if)
                    {
                        rtapi_print("initOffsetMap : unmatched token else \n");
                        throw std::exception();
                    }
                    sprintf(blockName, "%s#%s", subName, lastItem.second.c_str());
                    offset new_offset = {O_else, strstore(_setup.filename), lineOffset, lineNum - 1, -1};
                    _setup.offset_map.insert(std::make_pair(strstore(blockName), new_offset));
                }
                else if (isToken(script, &index, "elif"))
                {
                    if (_owordIdMap.size() <= 0)
                    {
                        rtapi_print("initOffsetMap : unmatched token elif\n");
                        throw std::exception();
                    }
                    std::pair<int, std::string> lastItem = _owordIdMap.back();
                    if (lastItem.first != O_if)
                    {
                        rtapi_print("initOffsetMap : unmatched token elif\n");
                        throw std::exception();
                    }

                    sprintf(blockName, "%s#%s", subName, lastItem.second.c_str());
                    offset new_offset = {O_elif, strstore(_setup.filename), lineOffset, lineNum - 1, -1};
                    _setup.offset_map.insert(std::make_pair(strstore(blockName), new_offset));
                }
                else
                {
                    is_token = false;
                }
                break;
            }
            case 'i': {
                if (isToken(script, &index, "if"))
                {
                    std::pair<int, std::string> ifItem = std::make_pair(O_if, std::to_string(lineNum - 1));
                    _owordIdMap.push_back(ifItem);

                    sprintf(blockName, "%s#%s", subName, ifItem.second.c_str());
                    offset new_offset = {O_if, strstore(_setup.filename), lineOffset, lineNum - 1, -1};
                    _setup.offset_map.insert(std::make_pair(strstore(blockName), new_offset));
                    // rtapi_print(">>>>>>>>>>>>>>>>>>>>>>>>>>blockName=%s , lineNum=%d  lineOffset=%ld \n",blockName,new_offset.sequence_number,new_offset.offset);
                }
                else
                {
                    is_token = false;
                }
                break;
            }
            case 'r': {
                if (isToken(script, &index, "return"))
                {
                    if (_owordIdMap.size() <= 0)
                    {
                        rtapi_print("intiOffsetMap:  unmathed token retrun \n");
                        throw std::exception();
                    }
                    std::pair<int, std::string> lastItem = _owordIdMap.back();
                    if (lastItem.first != O_sub)
                    {
                        rtapi_print("intiOffsetMap:  unmathed token retrun \n");
                        throw std::exception();
                    }
                    sprintf(blockName, "%s#%s", subName, lastItem.second.c_str());
                    offset new_offset = {O_return, strstore(_setup.filename), lineOffset, lineNum - 1, -1};
                    _setup.offset_map.insert(std::make_pair(strstore(blockName), new_offset));
                }
                else if (isToken(script, &index, "repeat"))
                {
                    std::string strId = std::to_string(lineNum);
                    std::pair<int, std::string> repeatItem = std::make_pair(O_repeat, strId);
                    _owordIdMap.push_back(repeatItem);

                    sprintf(blockName, "%s#%s", subName, strId.c_str());
                    offset new_offset = {O_repeat, strstore(_setup.filename), lineOffset, lineNum - 1, -1};
                    _setup.offset_map.insert(std::make_pair(strstore(blockName), new_offset));
                }
                else
                {
                    is_token = false;
                }
                break;
            }
            case 'w': {
                if (isToken(script, &index, "while"))
                {
                    std::string strId = std::to_string(lineNum - 1);
                    if (_owordIdMap.size() == 0 || _owordIdMap.back().first != O_do)
                    {
                        std::pair<int, std::string> whileItem = std::make_pair(O_while, strId);
                        _owordIdMap.push_back(whileItem);
                    }
                    else
                    {
                        _owordIdMap.pop_back();
                    }

                    sprintf(blockName, "%s#%s", subName, strId.c_str());
                    offset new_offset = {O_while, strstore(_setup.filename), lineOffset, lineNum - 1, -1};
                    _setup.offset_map.insert(std::make_pair(strstore(blockName), new_offset));
                }
                else
                {
                    is_token = false;
                }
                break;
            }
            default:
                is_token = false;
                break;
            }
            if (is_token == false)
            {
                rtapi_print("init offsetMap failed, is_token = false   %s \n", script);
                throw std::exception();
            }
        }
        //缺少end语法报错
        if (_owordIdMap.size() > 0)
        {
            std::pair<int, std::string> lastItem = _owordIdMap.back();
            int lineNum = std::atoi(lastItem.second.c_str()) + 1;
            _setup.current_line = lineNum;  // 用于报错指示行号.
            switch (lastItem.first)
            {
            case O_while:
                rtapi_print("intiOffsetMap:  mismatched end for while.   file:%s,  line: %d\n", filename.c_str(), lineNum);
                ERS(INTERP_ERR_SYNTAX_FAILED, "  mismatched end for while  ");
                _owordIdMap.pop_back();
                break;
            case O_if:
                rtapi_print("intiOffsetMap:  mismatched end for if.   file:%s,  line: %d\n", filename.c_str(), lineNum);
                ERS(INTERP_ERR_SYNTAX_FAILED, "  mismatched end for if  ");
                _owordIdMap.pop_back();
                break;
            default:
                rtapi_print("intiOffsetMap:  mismatched end for if.   file:%s,  line: %d\n", filename.c_str(), lineNum);
                ERS(INTERP_ERR_SYNTAX_FAILED, "  mismatched O_WORD: %d  ", lastItem.first);
                break;
            }
        }
    }
    catch (const std::exception& e)
    {
        rtapi_print("init offsetMap failed \n");
        _setup.offset_map.clear();
        std::cerr << e.what() << '\n';
        system("rm /tmp/blocks.jks");
        return INTERP_ERROR;  //  此处不使用ERS 返回错误码，init_offsetMap失败则按原解析流程继续解析，在后续解析过程中报错并返回行号。
    }
    // printf(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>initOffsetMap finished \n");
    // auto iter = _setup.offset_map.begin();
    // while (iter != std::end(_setup.offset_map))
    // {
    //     printf("blockName = %s\n", iter->first);
    //     printf("blockType=%d, offset = %ld, lineNum = %d\n", iter->second.type, iter->second.offset, iter->second.sequence_number);
    //     iter++;
    // }
    system("rm /tmp/blocks.jks");
    return INTERP_OK;
}

int Interp::updateSteppingState(int val)
{
    m_isSteppingMode = val;
    return INTERP_OK;
}

int Interp::reload_zucsettings()
{
    if (get_thread_type() != 0)
        return INTERP_ERROR;

    return restore_user_parameters();
}
